diff --git a/drivers/net/wireless/Kconfig b/drivers/net/wireless/Kconfig
index 3d1ffcb..bed2537 100644
--- a/drivers/net/wireless/Kconfig
+++ b/drivers/net/wireless/Kconfig
@@ -45,6 +45,7 @@ source "drivers/net/wireless/realtek/Kconfig"
 source "drivers/net/wireless/rsi/Kconfig"
 source "drivers/net/wireless/st/Kconfig"
 source "drivers/net/wireless/ti/Kconfig"
+source "drivers/net/wireless/xradio/Kconfig"
 source "drivers/net/wireless/zydas/Kconfig"
 source "drivers/net/wireless/quantenna/Kconfig"
 source "drivers/net/wireless/rtl8812au/Kconfig"
diff --git a/drivers/net/wireless/Makefile b/drivers/net/wireless/Makefile
index d0fbe9c..2442af0 100644
--- a/drivers/net/wireless/Makefile
+++ b/drivers/net/wireless/Makefile
@@ -18,6 +18,7 @@ obj-$(CONFIG_WLAN_VENDOR_REALTEK) += realtek/
 obj-$(CONFIG_WLAN_VENDOR_RSI) += rsi/
 obj-$(CONFIG_WLAN_VENDOR_ST) += st/
 obj-$(CONFIG_WLAN_VENDOR_TI) += ti/
+obj-$(CONFIG_WLAN_VENDOR_XRADIO) += xradio/
 obj-$(CONFIG_WLAN_VENDOR_ZYDAS) += zydas/
 obj-$(CONFIG_WLAN_VENDOR_QUANTENNA) += quantenna/
 
diff --git a/drivers/net/wireless/xradio/.7z b/drivers/net/wireless/xradio/.7z
new file mode 100644
index 0000000..4f8fc9c
Binary files /dev/null and b/drivers/net/wireless/xradio/.7z differ
diff --git a/drivers/net/wireless/xradio/Kconfig b/drivers/net/wireless/xradio/Kconfig
new file mode 100644
index 0000000..18d1e2b
--- /dev/null
+++ b/drivers/net/wireless/xradio/Kconfig
@@ -0,0 +1,46 @@
+config WLAN_VENDOR_XRADIO
+       tristate "XRADIO WLAN support"
+       depends on MAC80211
+       default m
+       help
+
+         This is an experimental driver for the XRADIO chip-set.
+         Enabling this option enables the generic driver without
+         any platform support.
+         Please select the appropriate platform below.
+
+if WLAN_VENDOR_XRADIO
+
+config XRADIO_NON_POWER_OF_TWO_BLOCKSIZES
+	bool "Platform supports non-power-of-two SDIO transfer"
+	depends on WLAN_VENDOR_XRADIO
+	default y
+	---help---
+	  Say N here only if you are running the driver on a platform
+	  which does not have support for non-power-of-two SDIO transfer.
+	  If unsure, say Y.
+
+config XRADIO_5GHZ_SUPPORT
+	bool "5GHz band support"
+	depends on WLAN_VENDOR_XRADIO
+	default n
+	---help---
+	  Say Y if your device supports 5GHz band. If unsure, say N.
+
+config XRADIO_WAPI_SUPPORT
+	bool "WAPI support"
+	depends on WLAN_VENDOR_XRADIO
+	default n
+	---help---
+	  Say Y if your compat-wireless support WAPI.
+	  If unsure, say N.
+
+config XRADIO_USE_EXTENSIONS
+	bool "Extensions for WFD and PS mode"
+	depends on WLAN_VENDOR_XRADIO
+	default y
+	---help---
+	  Say Y if you want to include XR extensions
+	  If unsure, say Y.
+
+endif
diff --git a/drivers/net/wireless/xradio/LICENSE b/drivers/net/wireless/xradio/LICENSE
new file mode 100644
index 0000000..23cb790
--- /dev/null
+++ b/drivers/net/wireless/xradio/LICENSE
@@ -0,0 +1,339 @@
+                    GNU GENERAL PUBLIC LICENSE
+                       Version 2, June 1991
+
+ Copyright (C) 1989, 1991 Free Software Foundation, Inc., <http://fsf.org/>
+ 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+                            Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+License is intended to guarantee your freedom to share and change free
+software--to make sure the software is free for all its users.  This
+General Public License applies to most of the Free Software
+Foundation's software and to any other program whose authors commit to
+using it.  (Some other Free Software Foundation software is covered by
+the GNU Lesser General Public License instead.)  You can apply it to
+your programs, too.
+
+  When we speak of free software, we are referring to freedom, not
+price.  Our General Public Licenses are designed to make sure that you
+have the freedom to distribute copies of free software (and charge for
+this service if you wish), that you receive source code or can get it
+if you want it, that you can change the software or use pieces of it
+in new free programs; and that you know you can do these things.
+
+  To protect your rights, we need to make restrictions that forbid
+anyone to deny you these rights or to ask you to surrender the rights.
+These restrictions translate to certain responsibilities for you if you
+distribute copies of the software, or if you modify it.
+
+  For example, if you distribute copies of such a program, whether
+gratis or for a fee, you must give the recipients all the rights that
+you have.  You must make sure that they, too, receive or can get the
+source code.  And you must show them these terms so they know their
+rights.
+
+  We protect your rights with two steps: (1) copyright the software, and
+(2) offer you this license which gives you legal permission to copy,
+distribute and/or modify the software.
+
+  Also, for each author's protection and ours, we want to make certain
+that everyone understands that there is no warranty for this free
+software.  If the software is modified by someone else and passed on, we
+want its recipients to know that what they have is not the original, so
+that any problems introduced by others will not reflect on the original
+authors' reputations.
+
+  Finally, any free program is threatened constantly by software
+patents.  We wish to avoid the danger that redistributors of a free
+program will individually obtain patent licenses, in effect making the
+program proprietary.  To prevent this, we have made it clear that any
+patent must be licensed for everyone's free use or not licensed at all.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.
+
+                    GNU GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License applies to any program or other work which contains
+a notice placed by the copyright holder saying it may be distributed
+under the terms of this General Public License.  The "Program", below,
+refers to any such program or work, and a "work based on the Program"
+means either the Program or any derivative work under copyright law:
+that is to say, a work containing the Program or a portion of it,
+either verbatim or with modifications and/or translated into another
+language.  (Hereinafter, translation is included without limitation in
+the term "modification".)  Each licensee is addressed as "you".
+
+Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running the Program is not restricted, and the output from the Program
+is covered only if its contents constitute a work based on the
+Program (independent of having been made by running the Program).
+Whether that is true depends on what the Program does.
+
+  1. You may copy and distribute verbatim copies of the Program's
+source code as you receive it, in any medium, provided that you
+conspicuously and appropriately publish on each copy an appropriate
+copyright notice and disclaimer of warranty; keep intact all the
+notices that refer to this License and to the absence of any warranty;
+and give any other recipients of the Program a copy of this License
+along with the Program.
+
+You may charge a fee for the physical act of transferring a copy, and
+you may at your option offer warranty protection in exchange for a fee.
+
+  2. You may modify your copy or copies of the Program or any portion
+of it, thus forming a work based on the Program, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) You must cause the modified files to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    b) You must cause any work that you distribute or publish, that in
+    whole or in part contains or is derived from the Program or any
+    part thereof, to be licensed as a whole at no charge to all third
+    parties under the terms of this License.
+
+    c) If the modified program normally reads commands interactively
+    when run, you must cause it, when started running for such
+    interactive use in the most ordinary way, to print or display an
+    announcement including an appropriate copyright notice and a
+    notice that there is no warranty (or else, saying that you provide
+    a warranty) and that users may redistribute the program under
+    these conditions, and telling the user how to view a copy of this
+    License.  (Exception: if the Program itself is interactive but
+    does not normally print such an announcement, your work based on
+    the Program is not required to print an announcement.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Program,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Program, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Program.
+
+In addition, mere aggregation of another work not based on the Program
+with the Program (or with a work based on the Program) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may copy and distribute the Program (or a work based on it,
+under Section 2) in object code or executable form under the terms of
+Sections 1 and 2 above provided that you also do one of the following:
+
+    a) Accompany it with the complete corresponding machine-readable
+    source code, which must be distributed under the terms of Sections
+    1 and 2 above on a medium customarily used for software interchange; or,
+
+    b) Accompany it with a written offer, valid for at least three
+    years, to give any third party, for a charge no more than your
+    cost of physically performing source distribution, a complete
+    machine-readable copy of the corresponding source code, to be
+    distributed under the terms of Sections 1 and 2 above on a medium
+    customarily used for software interchange; or,
+
+    c) Accompany it with the information you received as to the offer
+    to distribute corresponding source code.  (This alternative is
+    allowed only for noncommercial distribution and only if you
+    received the program in object code or executable form with such
+    an offer, in accord with Subsection b above.)
+
+The source code for a work means the preferred form of the work for
+making modifications to it.  For an executable work, complete source
+code means all the source code for all modules it contains, plus any
+associated interface definition files, plus the scripts used to
+control compilation and installation of the executable.  However, as a
+special exception, the source code distributed need not include
+anything that is normally distributed (in either source or binary
+form) with the major components (compiler, kernel, and so on) of the
+operating system on which the executable runs, unless that component
+itself accompanies the executable.
+
+If distribution of executable or object code is made by offering
+access to copy from a designated place, then offering equivalent
+access to copy the source code from the same place counts as
+distribution of the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  4. You may not copy, modify, sublicense, or distribute the Program
+except as expressly provided under this License.  Any attempt
+otherwise to copy, modify, sublicense or distribute the Program is
+void, and will automatically terminate your rights under this License.
+However, parties who have received copies, or rights, from you under
+this License will not have their licenses terminated so long as such
+parties remain in full compliance.
+
+  5. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Program or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Program (or any work based on the
+Program), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Program or works based on it.
+
+  6. Each time you redistribute the Program (or any work based on the
+Program), the recipient automatically receives a license from the
+original licensor to copy, distribute or modify the Program subject to
+these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties to
+this License.
+
+  7. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Program at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Program by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Program.
+
+If any portion of this section is held invalid or unenforceable under
+any particular circumstance, the balance of the section is intended to
+apply and the section as a whole is intended to apply in other
+circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system, which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  8. If the distribution and/or use of the Program is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Program under this License
+may add an explicit geographical distribution limitation excluding
+those countries, so that distribution is permitted only in or among
+countries not thus excluded.  In such case, this License incorporates
+the limitation as if written in the body of this License.
+
+  9. The Free Software Foundation may publish revised and/or new versions
+of the General Public License from time to time.  Such new versions will
+be similar in spirit to the present version, but may differ in detail to
+address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Program
+specifies a version number of this License which applies to it and "any
+later version", you have the option of following the terms and conditions
+either of that version or of any later version published by the Free
+Software Foundation.  If the Program does not specify a version number of
+this License, you may choose any version ever published by the Free Software
+Foundation.
+
+  10. If you wish to incorporate parts of the Program into other free
+programs whose distribution conditions are different, write to the author
+to ask for permission.  For software which is copyrighted by the Free
+Software Foundation, write to the Free Software Foundation; we sometimes
+make exceptions for this.  Our decision will be guided by the two goals
+of preserving the free status of all derivatives of our free software and
+of promoting the sharing and reuse of software generally.
+
+                            NO WARRANTY
+
+  11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY
+FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW.  EXCEPT WHEN
+OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES
+PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED
+OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.  THE ENTIRE RISK AS
+TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU.  SHOULD THE
+PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,
+REPAIR OR CORRECTION.
+
+  12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
+WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
+REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
+INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING
+OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED
+TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY
+YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER
+PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGES.
+
+                     END OF TERMS AND CONDITIONS
+
+            How to Apply These Terms to Your New Programs
+
+  If you develop a new program, and you want it to be of the greatest
+possible use to the public, the best way to achieve this is to make it
+free software which everyone can redistribute and change under these terms.
+
+  To do so, attach the following notices to the program.  It is safest
+to attach them to the start of each source file to most effectively
+convey the exclusion of warranty; and each file should have at least
+the "copyright" line and a pointer to where the full notice is found.
+
+    {description}
+    Copyright (C) {year}  {fullname}
+
+    This program is free software; you can redistribute it and/or modify
+    it under the terms of the GNU General Public License as published by
+    the Free Software Foundation; either version 2 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU General Public License for more details.
+
+    You 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.
+
+Also add information on how to contact you by electronic and paper mail.
+
+If the program is interactive, make it output a short notice like this
+when it starts in an interactive mode:
+
+    Gnomovision version 69, Copyright (C) year name of author
+    Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'.
+    This is free software, and you are welcome to redistribute it
+    under certain conditions; type `show c' for details.
+
+The hypothetical commands `show w' and `show c' should show the appropriate
+parts of the General Public License.  Of course, the commands you use may
+be called something other than `show w' and `show c'; they could even be
+mouse-clicks or menu items--whatever suits your program.
+
+You should also get your employer (if you work as a programmer) or your
+school, if any, to sign a "copyright disclaimer" for the program, if
+necessary.  Here is a sample; alter the names:
+
+  Yoyodyne, Inc., hereby disclaims all copyright interest in the program
+  `Gnomovision' (which makes passes at compilers) written by James Hacker.
+
+  {signature of Ty Coon}, 1 April 1989
+  Ty Coon, President of Vice
+
+This General Public License does not permit incorporating your program into
+proprietary programs.  If your program is a subroutine library, you may
+consider it more useful to permit linking proprietary applications with the
+library.  If this is what you want to do, use the GNU Lesser General
+Public License instead of this License.
diff --git a/drivers/net/wireless/xradio/Makefile b/drivers/net/wireless/xradio/Makefile
new file mode 100644
index 0000000..80a9a14
--- /dev/null
+++ b/drivers/net/wireless/xradio/Makefile
@@ -0,0 +1,54 @@
+# Standalone Makefile - uncomment for out-of-tree compilation
+#CONFIG_WLAN_VENDOR_XRADIO := m
+#CONFIG_XRADIO_USE_EXTENSIONS := y
+#CONFIG_XRADIO_WAPI_SUPPORT := n
+
+# Kernel part
+
+obj-$(CONFIG_WLAN_VENDOR_XRADIO) += xradio_wlan.o
+
+xradio_wlan-objs := \
+	fwio.o \
+	tx.o \
+	rx.o \
+	main.o \
+	queue.o \
+	hwio.o \
+	bh.o \
+	wsm.o \
+	sta.o \
+	ap.o \
+	keys.o \
+	scan.o \
+	module.o \
+	sdio.o \
+	pm.o \
+	ht.o \
+	p2p.o
+
+ccflags-$(CONFIG_WLAN_VENDOR_XRADIO) += -DMCAST_FWDING
+ccflags-$(CONFIG_WLAN_VENDOR_XRADIO) += -DXRADIO_SUSPEND_RESUME_FILTER_ENABLE
+ccflags-$(CONFIG_WLAN_VENDOR_XRADIO) += -DAP_AGGREGATE_FW_FIX
+ccflags-$(CONFIG_WLAN_VENDOR_XRADIO) += -DAP_HT_CAP_UPDATE
+ccflags-$(CONFIG_WLAN_VENDOR_XRADIO) += -DAP_HT_COMPAT_FIX
+ccflags-$(CONFIG_WLAN_VENDOR_XRADIO) += -DCONFIG_XRADIO_DUMP_ON_ERROR
+
+ccflags-$(CONFIG_WLAN_VENDOR_XRADIO) += -DCONFIG_XRADIO_SUSPEND_POWER_OFF
+
+# Extra IE for probe response from upper layer is needed in P2P GO
+# For offloading probe response to FW, the extra IE must be included
+# in the probe response template
+ccflags-$(CONFIG_WLAN_VENDOR_XRADIO) += -DPROBE_RESP_EXTRA_IE
+
+# Modified by wzw
+ccflags-$(CONFIG_WLAN_VENDOR_XRADIO) += -DTES_P2P_0002_ROC_RESTART
+ccflags-$(CONFIG_WLAN_VENDOR_XRADIO) += -DTES_P2P_000B_EXTEND_INACTIVITY_CNT
+ccflags-$(CONFIG_WLAN_VENDOR_XRADIO) += -DTES_P2P_000B_DISABLE_EAPOL_FILTER
+ccflags-$(CONFIG_WLAN_VENDOR_XRADIO) += -DXRADIO_USE_LONG_DTIM_PERIOD
+ccflags-$(CONFIG_WLAN_VENDOR_XRADIO) += -DXRADIO_USE_LONG_KEEP_ALIVE_PERIOD
+
+#ccflags-$(CONFIG_WLAN_VENDOR_XRADIO) += -DDEBUG
+#ccflags-$(CONFIG_WLAN_VENDOR_XRADIO) += -DXRADIO_DISABLE_HW_CRYPTO
+
+ldflags-$(CONFIG_WLAN_VENDOR_XRADIO) += --strip-debug
+
diff --git a/drivers/net/wireless/xradio/ap.c b/drivers/net/wireless/xradio/ap.c
new file mode 100644
index 0000000..5276c5a
--- /dev/null
+++ b/drivers/net/wireless/xradio/ap.c
@@ -0,0 +1,1624 @@
+/*
+ * STA and AP APIs for XRadio drivers
+ *
+ * Copyright (c) 2013, XRadio
+ * Author: XRadio
+ *
+ * This program is free software; you can 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 "xradio.h"
+#include "sta.h"
+#include "ap.h"
+#include "bh.h"
+#include "net/mac80211.h"
+
+#define XRADIO_LINK_ID_GC_TIMEOUT        ((unsigned long)(10 * HZ))
+#define XRADIO_ENABLE_ARP_FILTER_OFFLOAD  3
+
+#ifndef ERP_INFO_BYTE_OFFSET
+#define ERP_INFO_BYTE_OFFSET 2
+#endif
+
+static int xradio_upload_beacon(struct xradio_vif *priv);
+#ifdef PROBE_RESP_EXTRA_IE
+static int xradio_upload_proberesp(struct xradio_vif *priv);
+#endif
+static int xradio_upload_pspoll(struct xradio_vif *priv);
+static int xradio_upload_null(struct xradio_vif *priv);
+static int xradio_upload_qosnull(struct xradio_vif *priv);
+static int xradio_start_ap(struct xradio_vif *priv);
+static int xradio_update_beaconing(struct xradio_vif *priv);
+/*
+static int xradio_enable_beaconing(struct xradio_vif *priv,
+				   bool enable);
+*/
+static void __xradio_sta_notify(struct xradio_vif *priv,
+				enum sta_notify_cmd notify_cmd,
+				int link_id);
+
+/* ******************************************************************** */
+/* AP API */
+int xradio_sta_add(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+                   struct ieee80211_sta *sta)
+{
+	struct xradio_sta_priv *sta_priv = (struct xradio_sta_priv *)&sta->drv_priv;
+	struct xradio_vif *priv = xrwl_get_vif_from_ieee80211(vif);
+	struct xradio_link_entry *entry;
+	struct sk_buff *skb;
+#ifdef AP_AGGREGATE_FW_FIX
+	struct xradio_common *hw_priv = hw->priv;
+#endif
+
+	if (priv->mode != NL80211_IFTYPE_AP) {
+		return 0;
+	}
+
+	sta_priv->priv = priv;
+	sta_priv->link_id = xradio_find_link_id(priv, sta->addr);
+	if (WARN_ON(!sta_priv->link_id)) {
+		/* Impossible error */
+		wiphy_debug(hw->wiphy, "No more link IDs available.\n");
+		return -ENOENT;
+	}
+
+	entry = &priv->link_id_db[sta_priv->link_id - 1];
+	spin_lock_bh(&priv->ps_state_lock);
+	if ((sta->uapsd_queues & IEEE80211_WMM_IE_STA_QOSINFO_AC_MASK) ==
+	     IEEE80211_WMM_IE_STA_QOSINFO_AC_MASK) {
+		priv->sta_asleep_mask |= BIT(sta_priv->link_id);
+	}
+	entry->status = XRADIO_LINK_HARD;
+	while ((skb = skb_dequeue(&entry->rx_queue)))
+		ieee80211_rx_irqsafe(priv->hw, skb);
+	spin_unlock_bh(&priv->ps_state_lock);
+
+#ifdef AP_AGGREGATE_FW_FIX
+	hw_priv->connected_sta_cnt++;
+	if(hw_priv->connected_sta_cnt>1) {
+			wsm_lock_tx(hw_priv);
+			WARN_ON(wsm_set_block_ack_policy(hw_priv,
+					XRADIO_TX_BLOCK_ACK_DISABLED_FOR_ALL_TID,
+					XRADIO_RX_BLOCK_ACK_DISABLED_FOR_ALL_TID,
+					priv->if_id));
+			wsm_unlock_tx(hw_priv);
+	}
+#endif
+
+	return 0;
+}
+
+int xradio_sta_remove(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		      struct ieee80211_sta *sta)
+{
+	struct xradio_common *hw_priv = hw->priv;
+	struct xradio_sta_priv *sta_priv =
+			(struct xradio_sta_priv *)&sta->drv_priv;
+	struct xradio_vif *priv = xrwl_get_vif_from_ieee80211(vif);
+	struct xradio_link_entry *entry;
+
+	if (priv->mode != NL80211_IFTYPE_AP || !sta_priv->link_id) {
+		wiphy_warn(hw->wiphy, "no station to remove\n");
+		return 0;
+	}
+
+	entry = &priv->link_id_db[sta_priv->link_id - 1];
+	spin_lock_bh(&priv->ps_state_lock);
+	entry->status = XRADIO_LINK_RESERVE;
+	entry->timestamp = jiffies;
+	wsm_lock_tx_async(hw_priv);
+	if (queue_work(hw_priv->workqueue, &priv->link_id_work) <= 0)
+		wsm_unlock_tx(hw_priv);
+	spin_unlock_bh(&priv->ps_state_lock);
+	flush_workqueue(hw_priv->workqueue);
+
+#ifdef AP_AGGREGATE_FW_FIX
+	hw_priv->connected_sta_cnt--;
+	if(hw_priv->connected_sta_cnt <= 1) {
+		if ((priv->if_id != 1) ||
+			((priv->if_id == 1) && hw_priv->is_go_thru_go_neg)) {
+			wsm_lock_tx(hw_priv);
+			WARN_ON(wsm_set_block_ack_policy(hw_priv,
+						XRADIO_TX_BLOCK_ACK_ENABLED_FOR_ALL_TID,
+						XRADIO_RX_BLOCK_ACK_ENABLED_FOR_ALL_TID,
+						priv->if_id));
+			wsm_unlock_tx(hw_priv);
+		}
+	}
+#endif
+
+	return 0;
+}
+
+static void __xradio_sta_notify(struct xradio_vif *priv,
+				enum sta_notify_cmd notify_cmd,
+				int link_id)
+{
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	u32 bit, prev;
+
+	/* Zero link id means "for all link IDs" */
+	if (link_id)
+		bit = BIT(link_id);
+	else if (WARN_ON_ONCE(notify_cmd != STA_NOTIFY_AWAKE))
+		bit = 0;
+	else
+		bit = priv->link_id_map;
+	prev = priv->sta_asleep_mask & bit;
+
+	switch (notify_cmd) {
+	case STA_NOTIFY_SLEEP:
+		if (!prev) {
+			if (priv->buffered_multicasts &&
+					!priv->sta_asleep_mask)
+				queue_work(hw_priv->workqueue,
+					&priv->multicast_start_work);
+			priv->sta_asleep_mask |= bit;
+		}
+		break;
+	case STA_NOTIFY_AWAKE:
+		if (prev) {
+			priv->sta_asleep_mask &= ~bit;
+			priv->pspoll_mask &= ~bit;
+			if (priv->tx_multicast && link_id &&
+					!priv->sta_asleep_mask)
+				queue_work(hw_priv->workqueue,
+					&priv->multicast_stop_work);
+			xradio_bh_wakeup(hw_priv);
+		}
+		break;
+	}
+}
+
+void xradio_sta_notify(struct ieee80211_hw *dev,
+		       struct ieee80211_vif *vif,
+		       enum sta_notify_cmd notify_cmd,
+		       struct ieee80211_sta *sta)
+{
+	struct xradio_vif *priv = xrwl_get_vif_from_ieee80211(vif);
+	struct xradio_sta_priv *sta_priv = (struct xradio_sta_priv *)&sta->drv_priv;
+
+	spin_lock_bh(&priv->ps_state_lock);
+	__xradio_sta_notify(priv, notify_cmd, sta_priv->link_id);
+	spin_unlock_bh(&priv->ps_state_lock);
+}
+
+static void xradio_ps_notify(struct xradio_vif *priv,
+		      int link_id, bool ps)
+{
+	if (link_id > MAX_STA_IN_AP_MODE) {
+		ap_printk(XRADIO_DBG_WARN,"link_id is invalid=%d\n", link_id);
+		return;
+	}
+
+	ap_printk(XRADIO_DBG_NIY, "%s for LinkId: %d. STAs asleep: %.8X\n",
+	          ps ? "Stop" : "Start", link_id, priv->sta_asleep_mask);
+
+	/* TODO:COMBO: __xradio_sta_notify changed. */
+	__xradio_sta_notify(priv, ps ? STA_NOTIFY_SLEEP : STA_NOTIFY_AWAKE, link_id);
+}
+
+static int xradio_set_tim_impl(struct xradio_vif *priv, bool aid0_bit_set)
+{
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	struct sk_buff *skb;
+	struct wsm_update_ie update_ie = {
+		.what = WSM_UPDATE_IE_BEACON,
+		.count = 1,
+	};
+	u16 tim_offset, tim_length;
+	ap_printk(XRADIO_DBG_TRC,"%s\n", __FUNCTION__);
+	ap_printk(XRADIO_DBG_MSG, "%s mcast: %s.\n", __func__, 
+	          aid0_bit_set ? "ena" : "dis");
+
+	skb = ieee80211_beacon_get_tim(priv->hw, priv->vif, &tim_offset, &tim_length);
+	if (!skb) {
+		__xradio_flush(hw_priv, true, priv->if_id);
+		return -ENOENT;
+	}
+
+	if (tim_offset && tim_length >= 6) {
+		/* Ignore DTIM count from mac80211:
+		 * firmware handles DTIM internally. */
+		skb->data[tim_offset + 2] = 0;
+
+		/* Set/reset aid0 bit */
+		if (aid0_bit_set)
+			skb->data[tim_offset + 4] |= 1;
+		else
+			skb->data[tim_offset + 4] &= ~1;
+	}
+
+	update_ie.ies = &skb->data[tim_offset];
+	update_ie.length = tim_length;
+	//filter same tim info, yangfh
+	if(memcmp(priv->last_tim, update_ie.ies, tim_length)) {
+		WARN_ON(wsm_update_ie(hw_priv, &update_ie, priv->if_id));
+		memcpy(priv->last_tim, update_ie.ies, tim_length);
+		ap_printk(XRADIO_DBG_MSG,"%02x %02x %02x %02x %02x %02x\n", 
+		          update_ie.ies[0], update_ie.ies[1], update_ie.ies[2], 
+		          update_ie.ies[3], update_ie.ies[4], update_ie.ies[5]);
+	}
+
+	dev_kfree_skb(skb);
+
+	return 0;
+}
+
+void xradio_set_tim_work(struct work_struct *work)
+{
+	struct xradio_vif *priv = container_of(work, struct xradio_vif, set_tim_work);
+	xradio_set_tim_impl(priv, priv->aid0_bit_set);
+}
+
+int xradio_set_tim(struct ieee80211_hw *dev, struct ieee80211_sta *sta,
+		   bool set)
+{
+	struct xradio_sta_priv *sta_priv = (struct xradio_sta_priv *)&sta->drv_priv;
+	struct xradio_vif *priv = sta_priv->priv;
+
+	WARN_ON(priv->mode != NL80211_IFTYPE_AP);
+	queue_work(priv->hw_priv->workqueue, &priv->set_tim_work);
+	return 0;
+}
+
+void xradio_set_cts_work(struct work_struct *work)
+{
+	struct xradio_vif *priv =
+		container_of(work, struct xradio_vif, set_cts_work.work);
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	u8 erp_ie[3] = {WLAN_EID_ERP_INFO, 0x1, 0};
+	struct wsm_update_ie update_ie = {
+		.what = WSM_UPDATE_IE_BEACON,
+		.count = 1,
+		.ies = erp_ie,
+		.length = 3,
+	};
+	u32 erp_info;
+	__le32 use_cts_prot;
+
+	mutex_lock(&hw_priv->conf_mutex);
+	erp_info = priv->erp_info;
+	mutex_unlock(&hw_priv->conf_mutex);
+	use_cts_prot = (erp_info & WLAN_ERP_USE_PROTECTION)? __cpu_to_le32(1) : 0;
+
+	erp_ie[ERP_INFO_BYTE_OFFSET] = erp_info;
+
+	ap_printk(XRADIO_DBG_MSG, "ERP information 0x%x\n", erp_info);
+
+	/* TODO:COMBO: If 2 interfaces are on the same channel they share
+	the same ERP values */
+	WARN_ON(wsm_write_mib(hw_priv, WSM_MIB_ID_NON_ERP_PROTECTION,
+	                       &use_cts_prot, sizeof(use_cts_prot), priv->if_id));
+	/* If STA Mode update_ie is not required */
+	if (priv->mode != NL80211_IFTYPE_STATION) {
+		WARN_ON(wsm_update_ie(hw_priv, &update_ie, priv->if_id));
+	}
+
+	return;
+}
+
+static int xradio_set_btcoexinfo(struct xradio_vif *priv)
+{
+	struct wsm_override_internal_txrate arg;
+	int ret = 0;
+
+	if (priv->mode == NL80211_IFTYPE_STATION) {
+		/* Plumb PSPOLL and NULL template */
+		WARN_ON(xradio_upload_pspoll(priv));
+		WARN_ON(xradio_upload_null(priv));
+	} else {
+		return 0;
+	}
+
+	memset(&arg, 0, sizeof(struct wsm_override_internal_txrate));
+
+	if (!priv->vif->p2p) {
+		/* STATION mode */
+		if (priv->bss_params.operationalRateSet & ~0xF) {
+			ap_printk(XRADIO_DBG_NIY, "STA has ERP rates\n");
+			/* G or BG mode */
+			arg.internalTxRate = (__ffs(
+			priv->bss_params.operationalRateSet & ~0xF));
+		} else {
+			ap_printk(XRADIO_DBG_NIY, "STA has non ERP rates\n");
+			/* B only mode */
+			arg.internalTxRate = (__ffs(
+			priv->association_mode.basicRateSet));
+		}
+		arg.nonErpInternalTxRate = (__ffs(
+			priv->association_mode.basicRateSet));
+	} else {
+		/* P2P mode */
+		arg.internalTxRate = (__ffs(
+			priv->bss_params.operationalRateSet & ~0xF));
+		arg.nonErpInternalTxRate = (__ffs(
+			priv->bss_params.operationalRateSet & ~0xF));
+	}
+
+	ap_printk(XRADIO_DBG_NIY, "BTCOEX_INFO" "MODE %d, internalTxRate : %x,"
+	          "nonErpInternalTxRate: %x\n", priv->mode, arg.internalTxRate,
+	          arg.nonErpInternalTxRate);
+
+	ret = WARN_ON(wsm_write_mib(xrwl_vifpriv_to_hwpriv(priv),
+	               WSM_MIB_ID_OVERRIDE_INTERNAL_TX_RATE, 
+	               &arg, sizeof(arg), priv->if_id));
+
+	return ret;
+}
+
+void xradio_bss_info_changed(struct ieee80211_hw *dev,
+			     struct ieee80211_vif *vif,
+			     struct ieee80211_bss_conf *info,
+			     u32 changed)
+{
+	struct xradio_common *hw_priv = dev->priv;
+	struct xradio_vif *priv = xrwl_get_vif_from_ieee80211(vif);
+
+	mutex_lock(&hw_priv->conf_mutex);
+	if (changed & BSS_CHANGED_BSSID) {
+		memcpy(priv->bssid, info->bssid, ETH_ALEN);
+		xradio_setup_mac_pvif(priv);
+	}
+
+	/* TODO: BSS_CHANGED_IBSS */
+	if (changed & BSS_CHANGED_ARP_FILTER) {
+		struct wsm_arp_ipv4_filter filter = {0};
+		int i;
+		ap_printk(XRADIO_DBG_MSG, "[STA] BSS_CHANGED_ARP_FILTER cnt: %d\n",
+		          info->arp_addr_cnt);
+
+		if (info->arp_addr_cnt){
+			if (vif->type == NL80211_IFTYPE_STATION)
+				filter.enable = (u32)XRADIO_ENABLE_ARP_FILTER_OFFLOAD;
+			else if (priv->join_status == XRADIO_JOIN_STATUS_AP)
+				filter.enable = (u32)(1<<1);
+			else
+				filter.enable = 0;
+		}
+
+		/* Currently only one IP address is supported by firmware.
+		 * In case of more IPs arp filtering will be disabled. */
+		if (info->arp_addr_cnt > 0 &&
+		    info->arp_addr_cnt <= WSM_MAX_ARP_IP_ADDRTABLE_ENTRIES) {
+			for (i = 0; i < info->arp_addr_cnt; i++) {
+				filter.ipv4Address[i] = info->arp_addr_list[i];
+				ap_printk(XRADIO_DBG_NIY, "[STA]addr[%d]: 0x%X\n", i, filter.ipv4Address[i]);
+			}
+		} else
+			filter.enable = 0;
+
+		if (filter.enable)
+			xradio_set_arpreply(dev, vif);
+
+		priv->filter4.enable = filter.enable;
+		ap_printk(XRADIO_DBG_NIY, "[STA]arp ip filter enable: %d\n", __le32_to_cpu(filter.enable));
+
+		if (wsm_set_arp_ipv4_filter(hw_priv, &filter, priv->if_id))
+			WARN_ON(1);
+
+		if (filter.enable &&
+			(priv->join_status == XRADIO_JOIN_STATUS_STA)) {
+			/* Firmware requires that value for this 1-byte field must
+			 * be specified in units of 500us. Values above the 128ms
+			 * threshold are not supported. */
+			//if (info->dynamic_ps_timeout >= 0x80)
+			//	priv->powersave_mode.fastPsmIdlePeriod = 0xFF;
+			//else
+			//	priv->powersave_mode.fastPsmIdlePeriod = info->dynamic_ps_timeout << 1;
+
+			priv->powersave_mode.fastPsmIdlePeriod = 200;//when connected,the dev->conf.dynamic_ps_timeout value is 0
+			priv->powersave_mode.apPsmChangePeriod = 200; //100ms, add by yangfh
+			ap_printk(XRADIO_DBG_NIY, "[STA]fastPsmIdle=%d, apPsmChange=%d\n", 
+			          priv->powersave_mode.fastPsmIdlePeriod, 
+			          priv->powersave_mode.apPsmChangePeriod);
+
+			if (priv->setbssparams_done) {
+				int ret = 0;
+				struct wsm_set_pm pm = priv->powersave_mode;
+				if (priv->user_power_set_true)
+					priv->powersave_mode.pmMode = priv->user_pm_mode;
+				else if ((priv->power_set_true &&
+				         ((priv->powersave_mode.pmMode == WSM_PSM_ACTIVE) ||
+				         (priv->powersave_mode.pmMode == WSM_PSM_PS)))    ||
+				         !priv->power_set_true)
+					priv->powersave_mode.pmMode = WSM_PSM_FAST_PS;
+
+				ret = xradio_set_pm (priv, &priv->powersave_mode);
+				if(ret)
+					priv->powersave_mode = pm;
+			} else {
+				priv->powersave_mode.pmMode = WSM_PSM_FAST_PS;
+			}
+			priv->power_set_true = 0;
+			priv->user_power_set_true = 0;
+		}
+	}
+
+	if (changed & BSS_CHANGED_BEACON) {
+		ap_printk(XRADIO_DBG_NIY, "BSS_CHANGED_BEACON\n");
+#ifdef HIDDEN_SSID
+		if(priv->join_status != XRADIO_JOIN_STATUS_AP) {
+			priv->hidden_ssid = info->hidden_ssid;
+			priv->ssid_length = info->ssid_len;
+			memcpy(priv->ssid, info->ssid, info->ssid_len);
+		} else
+			ap_printk(XRADIO_DBG_NIY, "priv->join_status=%d\n", priv->join_status);
+#endif
+		WARN_ON(xradio_upload_beacon(priv));
+		WARN_ON(xradio_update_beaconing(priv));
+	}
+
+	if (changed & BSS_CHANGED_BEACON_ENABLED) {
+		ap_printk(XRADIO_DBG_NIY, "BSS_CHANGED_BEACON_ENABLED dummy\n");
+		priv->enable_beacon = info->enable_beacon;
+	}
+
+	if (changed & BSS_CHANGED_BEACON_INT) {
+		ap_printk(XRADIO_DBG_NIY, "CHANGED_BEACON_INT\n");
+		/* Restart AP only when connected */
+		if (priv->join_status == XRADIO_JOIN_STATUS_AP)
+			WARN_ON(xradio_update_beaconing(priv));
+	}
+
+
+	if (changed & BSS_CHANGED_ASSOC) {
+		wsm_lock_tx(hw_priv);
+		priv->wep_default_key_id = -1;
+		wsm_unlock_tx(hw_priv);
+
+		if (!info->assoc /* && !info->ibss_joined */) {
+			priv->cqm_link_loss_count = XRADIO_LINK_LOSS_THOLD_DEF;
+			priv->cqm_beacon_loss_count = XRADIO_BSS_LOSS_THOLD_DEF;
+			priv->cqm_tx_failure_thold = 0;
+		}
+		priv->cqm_tx_failure_count = 0;
+	}
+
+	if (changed & 
+	    (BSS_CHANGED_ASSOC        |
+	     BSS_CHANGED_BASIC_RATES  |
+	     BSS_CHANGED_ERP_PREAMBLE |
+	     BSS_CHANGED_HT           |
+	     BSS_CHANGED_ERP_SLOT)) {
+		int is_combo = 0;
+		int i;
+		struct xradio_vif *tmp_priv;
+		ap_printk(XRADIO_DBG_NIY, "BSS_CHANGED_ASSOC.\n");
+		if (info->assoc) { /* TODO: ibss_joined */
+			struct ieee80211_sta *sta = NULL;
+			if (info->dtim_period)
+				priv->join_dtim_period = info->dtim_period;
+			priv->beacon_int = info->beacon_int;
+
+			/* Associated: kill join timeout */
+			cancel_delayed_work_sync(&priv->join_timeout);
+
+			rcu_read_lock();
+			if (info->bssid)
+				sta = ieee80211_find_sta(vif, info->bssid);
+			if (sta) {
+				/* TODO:COMBO:Change this once
+				* mac80211 changes are available */
+				BUG_ON(!hw_priv->channel);
+				hw_priv->ht_oper.ht_cap = sta->ht_cap;
+				priv->bss_params.operationalRateSet =__cpu_to_le32(
+				  xradio_rate_mask_to_wsm(hw_priv, sta->supp_rates[hw_priv->channel->band]));
+				/* TODO by Icenowy: I think this may lead to some problems. */
+//				hw_priv->ht_oper.channel_type   = info->channel_type;
+				hw_priv->ht_oper.operation_mode = info->ht_operation_mode;
+			} else {
+				memset(&hw_priv->ht_oper, 0, sizeof(hw_priv->ht_oper));
+				priv->bss_params.operationalRateSet = -1;
+			}
+			rcu_read_unlock();
+			priv->htcap = (sta && xradio_is_ht(&hw_priv->ht_oper));
+			xradio_for_each_vif(hw_priv, tmp_priv, i) {
+				if (!tmp_priv)
+					continue;
+				if (tmp_priv->join_status >= XRADIO_JOIN_STATUS_STA)
+					is_combo++;
+			}
+
+			if (is_combo > 1) {
+				hw_priv->vif0_throttle = XRWL_HOST_VIF0_11BG_THROTTLE;
+				hw_priv->vif1_throttle = XRWL_HOST_VIF1_11BG_THROTTLE;
+				ap_printk(XRADIO_DBG_WARN, "%sASSOC is_combo %d\n", 
+				         (priv->join_status == XRADIO_JOIN_STATUS_STA)?"[STA] ":"",
+				          hw_priv->vif0_throttle);
+			} else if ((priv->join_status == XRADIO_JOIN_STATUS_STA) && priv->htcap) {
+				hw_priv->vif0_throttle = XRWL_HOST_VIF0_11N_THROTTLE;
+				hw_priv->vif1_throttle = XRWL_HOST_VIF1_11N_THROTTLE;
+				ap_printk(XRADIO_DBG_WARN, "[STA] ASSOC HTCAP 11N %d\n",hw_priv->vif0_throttle);
+			} else {
+				hw_priv->vif0_throttle = XRWL_HOST_VIF0_11BG_THROTTLE;
+				hw_priv->vif1_throttle = XRWL_HOST_VIF1_11BG_THROTTLE;
+				ap_printk(XRADIO_DBG_WARN, "ASSOC not_combo 11BG %d\n",hw_priv->vif0_throttle);
+			}
+
+			if (sta) {
+				__le32 val = 0;
+				if (hw_priv->ht_oper.operation_mode & IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT) {
+					ap_printk(XRADIO_DBG_NIY,"[STA] Non-GF STA present\n");
+					/* Non Green field capable STA */
+					val = __cpu_to_le32(BIT(1));
+				}
+				WARN_ON(wsm_write_mib(hw_priv, WSM_MID_ID_SET_HT_PROTECTION,
+				                       &val, sizeof(val), priv->if_id));
+			}
+
+			priv->association_mode.greenfieldMode = xradio_ht_greenfield(&hw_priv->ht_oper);
+			priv->association_mode.flags =
+			  WSM_ASSOCIATION_MODE_SNOOP_ASSOC_FRAMES |
+			  WSM_ASSOCIATION_MODE_USE_PREAMBLE_TYPE  |
+			  WSM_ASSOCIATION_MODE_USE_HT_MODE        |
+			  WSM_ASSOCIATION_MODE_USE_BASIC_RATE_SET |
+			  WSM_ASSOCIATION_MODE_USE_MPDU_START_SPACING;
+
+			priv->association_mode.preambleType =
+			  (info->use_short_preamble ? WSM_JOIN_PREAMBLE_SHORT : WSM_JOIN_PREAMBLE_LONG);
+			priv->association_mode.basicRateSet = __cpu_to_le32(
+			  xradio_rate_mask_to_wsm(hw_priv,info->basic_rates));
+			priv->association_mode.mpduStartSpacing =
+			  xradio_ht_ampdu_density(&hw_priv->ht_oper);
+
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+			//priv->cqm_beacon_loss_count = info->cqm_beacon_miss_thold;
+			//priv->cqm_tx_failure_thold  = info->cqm_tx_fail_thold;
+			//priv->cqm_tx_failure_count  = 0;
+			cancel_delayed_work_sync(&priv->bss_loss_work);
+			cancel_delayed_work_sync(&priv->connection_loss_work);
+#endif /* CONFIG_XRADIO_USE_EXTENSIONS */
+
+			priv->bss_params.beaconLostCount = (priv->cqm_beacon_loss_count ?
+			  priv->cqm_beacon_loss_count : priv->cqm_link_loss_count);
+
+			priv->bss_params.aid = info->aid;
+
+			if (priv->join_dtim_period < 1)
+				priv->join_dtim_period = 1;
+
+			ap_printk(XRADIO_DBG_MSG, "[STA] DTIM %d, interval: %d\n",
+			          priv->join_dtim_period, priv->beacon_int);
+			ap_printk(XRADIO_DBG_MSG, "[STA] Preamble: %d, " \
+			          "Greenfield: %d, Aid: %d, Rates: 0x%.8X, Basic: 0x%.8X\n",
+			          priv->association_mode.preambleType,
+			          priv->association_mode.greenfieldMode,
+			          priv->bss_params.aid,
+			          priv->bss_params.operationalRateSet,
+			          priv->association_mode.basicRateSet);
+			WARN_ON(wsm_set_association_mode(hw_priv, &priv->association_mode, priv->if_id));
+			WARN_ON(wsm_keep_alive_period(hw_priv, XRADIO_KEEP_ALIVE_PERIOD /* sec */,
+			                               priv->if_id));
+			WARN_ON(wsm_set_bss_params(hw_priv, &priv->bss_params, priv->if_id));
+			priv->setbssparams_done = true;
+#ifdef XRADIO_USE_LONG_DTIM_PERIOD
+{
+			int join_dtim_period_extend;
+			if (priv->join_dtim_period <= 3) {
+				join_dtim_period_extend = priv->join_dtim_period * 3;
+			} else if (priv->join_dtim_period <= 5) {
+				join_dtim_period_extend = priv->join_dtim_period * 2;
+			} else {
+				join_dtim_period_extend = priv->join_dtim_period;
+			}
+			WARN_ON(wsm_set_beacon_wakeup_period(hw_priv,
+				((priv->beacon_int * join_dtim_period_extend) > MAX_BEACON_SKIP_TIME_MS 
+				? 1 : join_dtim_period_extend) , 0, priv->if_id));
+}
+#else
+			WARN_ON(wsm_set_beacon_wakeup_period(hw_priv,
+				((priv->beacon_int * priv->join_dtim_period) > MAX_BEACON_SKIP_TIME_MS 
+				? 1 : priv->join_dtim_period) , 0, priv->if_id));
+#endif
+			if (priv->htcap) {
+				wsm_lock_tx(hw_priv);
+				/* Statically enabling block ack for TX/RX */
+				WARN_ON(wsm_set_block_ack_policy(hw_priv, hw_priv->ba_tid_mask,
+				                                  hw_priv->ba_tid_mask, priv->if_id));
+				wsm_unlock_tx(hw_priv);
+			}
+			/*set ps active,avoid that when connecting process,the device sleeps,then can't receive pkts.*/
+			if (changed & BSS_CHANGED_ASSOC) 
+				priv->powersave_mode.pmMode = WSM_PSM_ACTIVE;
+			xradio_set_pm(priv, &priv->powersave_mode);
+			if (priv->vif->p2p) {
+				ap_printk(XRADIO_DBG_NIY, "[STA] Setting p2p powersave configuration.\n");
+				WARN_ON(wsm_set_p2p_ps_modeinfo(hw_priv, &priv->p2p_ps_modeinfo, priv->if_id));
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+				//xradio_notify_noa(priv, XRADIO_NOA_NOTIFICATION_DELAY);
+#endif
+			}
+
+			if (priv->mode == NL80211_IFTYPE_STATION)
+				WARN_ON(xradio_upload_qosnull(priv));
+
+			if (hw_priv->is_BT_Present)
+				WARN_ON(xradio_set_btcoexinfo(priv));
+#if 0
+			/* It's better to override internal TX rete; otherwise
+			 * device sends RTS at too high rate. However device
+			 * can't receive CTS at 1 and 2 Mbps. Well, 5.5 is a
+			 * good choice for RTS/CTS, but that means PS poll
+			 * will be sent at the same rate - impact on link
+			 * budget. Not sure what is better.. */
+
+			/* Update: internal rate selection algorythm is not
+			 * bad: if device is not receiving CTS at high rate,
+			 * it drops RTS rate.
+			 * So, conclusion: if-0 the code. Keep code just for
+			 * information:
+			 * Do not touch WSM_MIB_ID_OVERRIDE_INTERNAL_TX_RATE! */
+
+			/* ~3 is a bug in device: RTS/CTS is not working at
+			 * low rates */
+			__le32 internal_tx_rate = __cpu_to_le32(
+			                          __ffs(priv->association_mode.basicRateSet & ~3));
+			WARN_ON(wsm_write_mib(priv, WSM_MIB_ID_OVERRIDE_INTERNAL_TX_RATE,
+			                       &internal_tx_rate,sizeof(internal_tx_rate)));
+#endif
+		} else {
+			memset(&priv->association_mode, 0, sizeof(priv->association_mode));
+			memset(&priv->bss_params, 0, sizeof(priv->bss_params));
+		}
+	}
+	if (changed & (BSS_CHANGED_ASSOC | BSS_CHANGED_ERP_CTS_PROT)) {
+		u32 prev_erp_info = priv->erp_info;
+		if (priv->join_status == XRADIO_JOIN_STATUS_AP) {
+			if (info->use_cts_prot)
+				priv->erp_info |= WLAN_ERP_USE_PROTECTION;
+			else if (!(prev_erp_info & WLAN_ERP_NON_ERP_PRESENT))
+				priv->erp_info &= ~WLAN_ERP_USE_PROTECTION;
+
+			if (prev_erp_info != priv->erp_info)
+				queue_delayed_work(hw_priv->workqueue, &priv->set_cts_work, 0*HZ);
+		}
+	}
+
+	if (changed & (BSS_CHANGED_ASSOC | BSS_CHANGED_ERP_SLOT)) {
+		__le32 slot_time = info->use_short_slot ? __cpu_to_le32(9) : __cpu_to_le32(20);
+		ap_printk(XRADIO_DBG_MSG, "[STA] Slot time :%d us.\n", __le32_to_cpu(slot_time));
+		WARN_ON(wsm_write_mib(hw_priv, WSM_MIB_ID_DOT11_SLOT_TIME, &slot_time,
+		                       sizeof(slot_time), priv->if_id));
+	}
+	if (changed & (BSS_CHANGED_ASSOC | BSS_CHANGED_CQM)) {
+		struct wsm_rcpi_rssi_threshold threshold = {
+			.rollingAverageCount = 8,
+		};
+
+#if 0
+		/* For verification purposes */
+		info->cqm_rssi_thold = -50;
+		info->cqm_rssi_hyst = 4;
+#endif /* 0 */
+
+		ap_printk(XRADIO_DBG_NIY, "[CQM] RSSI threshold subscribe: %d(+-%d)\n",
+		         info->cqm_rssi_thold, info->cqm_rssi_hyst);
+
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+		priv->cqm_rssi_thold = info->cqm_rssi_thold;
+		priv->cqm_rssi_hyst  = info->cqm_rssi_hyst;
+#endif /* CONFIG_XRADIO_USE_EXTENSIONS */
+
+		if (info->cqm_rssi_thold || info->cqm_rssi_hyst) {
+			/* RSSI subscription enabled */
+			/* TODO: It's not a correct way of setting threshold.
+			 * Upper and lower must be set equal here and adjusted
+			 * in callback. However current implementation is much
+			 * more relaible and stable. */
+			if (priv->cqm_use_rssi) {
+				threshold.upperThreshold = info->cqm_rssi_thold + info->cqm_rssi_hyst;
+				threshold.lowerThreshold = info->cqm_rssi_thold;
+			} else {
+				/* convert RSSI to RCPI, RCPI = (RSSI + 110) * 2 */
+				threshold.upperThreshold = (info->cqm_rssi_thold + info->cqm_rssi_hyst + 110)<<1;
+				threshold.lowerThreshold = (info->cqm_rssi_thold + 110)<<1;
+			}
+			threshold.rssiRcpiMode |= WSM_RCPI_RSSI_THRESHOLD_ENABLE;
+		} else {
+			/* There is a bug in FW, see sta.c. We have to enable
+			 * dummy subscription to get correct RSSI values. */
+			threshold.rssiRcpiMode |= WSM_RCPI_RSSI_THRESHOLD_ENABLE |
+			                          WSM_RCPI_RSSI_DONT_USE_UPPER   |
+			                          WSM_RCPI_RSSI_DONT_USE_LOWER;
+		}
+		WARN_ON(wsm_set_rcpi_rssi_threshold(hw_priv, &threshold, priv->if_id));
+
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+		//priv->cqm_tx_failure_thold = info->cqm_tx_fail_thold;
+		//priv->cqm_tx_failure_count = 0;
+
+		//if (priv->cqm_beacon_loss_count != info->cqm_beacon_miss_thold) {
+		//	priv->cqm_beacon_loss_count = info->cqm_beacon_miss_thold;
+		//	priv->bss_params.beaconLostCount = (priv->cqm_beacon_loss_count?
+		//	  priv->cqm_beacon_loss_count : priv->cqm_link_loss_count);
+			/* Make sure we are associated before sending
+			 * set_bss_params to firmware */
+			if (priv->bss_params.aid) {
+				WARN_ON(wsm_set_bss_params(hw_priv, &priv->bss_params, priv->if_id));
+				priv->setbssparams_done = true;
+			}
+		//}
+#endif /* CONFIG_XRADIO_USE_EXTENSIONS */
+	}
+	/*
+	 * in linux3.4 mac,the  enum ieee80211_bss_change variable doesn't have
+	 * BSS_CHANGED_PS and BSS_CHANGED_RETRY_LIMITS enum value.
+	 */
+#if 0
+	if (changed & BSS_CHANGED_PS) {
+		if (info->ps_enabled == false)
+			priv->powersave_mode.pmMode = WSM_PSM_ACTIVE;
+		else if (info->dynamic_ps_timeout <= 0)
+			priv->powersave_mode.pmMode = WSM_PSM_PS;
+		else
+			priv->powersave_mode.pmMode = WSM_PSM_FAST_PS;
+
+		ap_printk(XRADIO_DBG_MSG, "[STA] Aid: %d, Joined: %s, Powersave: %s\n",
+		          priv->bss_params.aid,
+		          priv->join_status == XRADIO_JOIN_STATUS_STA ? "yes" : "no",
+		         (priv->powersave_mode.pmMode == WSM_PSM_ACTIVE ? "WSM_PSM_ACTIVE" :
+		          priv->powersave_mode.pmMode == WSM_PSM_PS ? "WSM_PSM_PS" :
+		          priv->powersave_mode.pmMode == WSM_PSM_FAST_PS ? "WSM_PSM_FAST_PS" :
+		          "UNKNOWN"));
+
+		/* Firmware requires that value for this 1-byte field must
+		 * be specified in units of 500us. Values above the 128ms
+		 * threshold are not supported. */
+		if (info->dynamic_ps_timeout >= 0x80)
+			priv->powersave_mode.fastPsmIdlePeriod = 0xFF;
+		else
+			priv->powersave_mode.fastPsmIdlePeriod = info->dynamic_ps_timeout << 1;
+		ap_printk(XRADIO_DBG_NIY, "[STA]CHANGED_PS fastPsmIdle=%d, apPsmChange=%d\n", 
+		          priv->powersave_mode.fastPsmIdlePeriod, 
+		          priv->powersave_mode.apPsmChangePeriod);
+
+		if (priv->join_status == XRADIO_JOIN_STATUS_STA && priv->bss_params.aid &&
+			  priv->setbssparams_done && priv->filter4.enable)
+			xradio_set_pm(priv, &priv->powersave_mode);
+		else
+			priv->power_set_true = 1;
+	}
+
+	if (changed & BSS_CHANGED_RETRY_LIMITS) {
+		ap_printk(XRADIO_DBG_NIY, "Retry limits: %d (long), %d (short).\n", 
+		          info->retry_long, info->retry_short);
+		spin_lock_bh(&hw_priv->tx_policy_cache.lock);
+		/*TODO:COMBO: for now it's still handled per hw and kept
+		 * in xradio_common */
+		hw_priv->long_frame_max_tx_count  = info->retry_long;
+		hw_priv->short_frame_max_tx_count = 
+		  (info->retry_short < 0x0F ? info->retry_short : 0x0F);
+		hw_priv->hw->max_rate_tries = hw_priv->short_frame_max_tx_count;
+		spin_unlock_bh(&hw_priv->tx_policy_cache.lock);
+		/* TBD: I think we don't need tx_policy_force_upload().
+		 * Outdated policies will leave cache in a normal way. */
+		/* WARN_ON(tx_policy_force_upload(priv)); */
+	}
+#endif
+	/*in linux3.4 mac,the  enum ieee80211_bss_change variable doesn't have BSS_CHANGED_P2P_PS enum value*/
+#if 0
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+	if (changed & BSS_CHANGED_P2P_PS) {
+		struct wsm_p2p_ps_modeinfo *modeinfo;
+		modeinfo = &priv->p2p_ps_modeinfo;
+		ap_printk(XRADIO_DBG_NIY, "[AP] BSS_CHANGED_P2P_PS\n");
+		ap_printk(XRADIO_DBG_NIY, "[AP] Legacy PS: %d for AID %d in %d mode.\n",
+		          info->p2p_ps.legacy_ps, priv->bss_params.aid, priv->join_status);
+
+		if (info->p2p_ps.legacy_ps >= 0) {
+			if (info->p2p_ps.legacy_ps > 0)
+				priv->powersave_mode.pmMode = WSM_PSM_PS;
+			else
+				priv->powersave_mode.pmMode = WSM_PSM_ACTIVE;
+
+			if(info->p2p_ps.ctwindow && info->p2p_ps.opp_ps)
+				priv->powersave_mode.pmMode = WSM_PSM_PS;
+			if (priv->join_status == XRADIO_JOIN_STATUS_STA)
+				xradio_set_pm(priv, &priv->powersave_mode);
+		}
+
+		ap_printk(XRADIO_DBG_MSG, "[AP] CTWindow: %d\n", info->p2p_ps.ctwindow);
+		if (info->p2p_ps.ctwindow >= 128)
+			modeinfo->oppPsCTWindow = 127;
+		else if (info->p2p_ps.ctwindow >= 0)
+			modeinfo->oppPsCTWindow = info->p2p_ps.ctwindow;
+
+		ap_printk(XRADIO_DBG_MSG, "[AP] Opportunistic: %d\n", info->p2p_ps.opp_ps);
+		switch (info->p2p_ps.opp_ps) {
+		case 0:
+			modeinfo->oppPsCTWindow &= ~(BIT(7));
+			break;
+		case 1:
+			modeinfo->oppPsCTWindow |= BIT(7);
+			break;
+		default:
+			break;
+		}
+
+		ap_printk(XRADIO_DBG_MSG, "[AP] NOA: %d, %d, %d, %d\n",
+		          info->p2p_ps.count, info->p2p_ps.start,
+		          info->p2p_ps.duration, info->p2p_ps.interval);
+		/* Notice of Absence */
+		modeinfo->count = info->p2p_ps.count;
+
+		if (info->p2p_ps.count) {
+			/* In case P2P_GO we need some extra time to be sure
+			 * we will update beacon/probe_resp IEs correctly */
+#define NOA_DELAY_START_MS	300
+			if (priv->join_status == XRADIO_JOIN_STATUS_AP)
+				modeinfo->startTime = __cpu_to_le32(info->p2p_ps.start + NOA_DELAY_START_MS);
+			else
+				modeinfo->startTime = __cpu_to_le32(info->p2p_ps.start);
+			modeinfo->duration    = __cpu_to_le32(info->p2p_ps.duration);
+			modeinfo->interval    = __cpu_to_le32(info->p2p_ps.interval);
+			modeinfo->dtimCount   = 1;
+			modeinfo->reserved    = 0;
+		} else {
+			modeinfo->dtimCount = 0;
+			modeinfo->startTime = 0;
+			modeinfo->reserved  = 0;
+			modeinfo->duration  = 0;
+			modeinfo->interval  = 0;
+		}
+
+#if defined(CONFIG_XRADIO_DEBUG)
+		print_hex_dump_bytes("p2p_set_ps_modeinfo: ", DUMP_PREFIX_NONE,
+		                     (u8 *)modeinfo, sizeof(*modeinfo));
+#endif /* CONFIG_XRADIO_DEBUG */
+
+		if (priv->join_status == XRADIO_JOIN_STATUS_STA ||
+		    priv->join_status == XRADIO_JOIN_STATUS_AP) {
+			WARN_ON(wsm_set_p2p_ps_modeinfo(hw_priv, modeinfo, priv->if_id));
+		}
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+		/* Temporary solution while firmware don't support NOA change
+		 * notification yet */
+		xradio_notify_noa(priv, 10);
+#endif
+	}
+#endif /* CONFIG_XRADIO_USE_EXTENSIONS */
+#endif
+	mutex_unlock(&hw_priv->conf_mutex);
+}
+
+void xradio_multicast_start_work(struct work_struct *work)
+{
+	struct xradio_vif *priv =
+	       container_of(work, struct xradio_vif, multicast_start_work);
+	long tmo = priv->join_dtim_period * (priv->beacon_int + 20) * HZ / 1024;
+
+	cancel_work_sync(&priv->multicast_stop_work);
+	if (!priv->aid0_bit_set) {
+		wsm_lock_tx(priv->hw_priv);
+		xradio_set_tim_impl(priv, true);
+		priv->aid0_bit_set = true;
+		mod_timer(&priv->mcast_timeout, jiffies + tmo);
+		wsm_unlock_tx(priv->hw_priv);
+	}
+}
+
+void xradio_multicast_stop_work(struct work_struct *work)
+{
+	struct xradio_vif *priv =
+		container_of(work, struct xradio_vif, multicast_stop_work);
+
+	if (priv->aid0_bit_set) {
+		del_timer_sync(&priv->mcast_timeout);
+		wsm_lock_tx(priv->hw_priv);
+		priv->aid0_bit_set = false;
+		xradio_set_tim_impl(priv, false);
+		wsm_unlock_tx(priv->hw_priv);
+	}
+}
+
+void xradio_mcast_timeout(struct timer_list *t)
+{
+	struct xradio_vif *priv = from_timer(priv, t, mcast_timeout);
+
+	ap_printk(XRADIO_DBG_WARN, "Multicast delivery timeout.\n");
+	spin_lock_bh(&priv->ps_state_lock);
+	priv->tx_multicast = priv->aid0_bit_set && priv->buffered_multicasts;
+	if (priv->tx_multicast)
+		xradio_bh_wakeup(xrwl_vifpriv_to_hwpriv(priv));
+	spin_unlock_bh(&priv->ps_state_lock);
+}
+
+int xradio_ampdu_action(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+                        struct ieee80211_ampdu_params *params)
+{
+	/* Aggregation is implemented fully in firmware,
+	 * including block ack negotiation.
+	 * In case of AMPDU aggregation in RX direction
+	 * re-ordering of packets takes place on host. mac80211
+	 * needs the ADDBA Request to setup reodering.mac80211 also
+	 * sends ADDBA Response which is discarded in the driver as
+	 * FW generates the ADDBA Response on its own.*/
+	int ret;
+
+	switch (params->action) {
+	case IEEE80211_AMPDU_RX_START:
+	case IEEE80211_AMPDU_RX_STOP:
+		/* Just return OK to mac80211 */
+		ret = 0;
+		break;
+	default:
+		ret = -ENOTSUPP;
+	}
+	return ret;
+}
+
+/* ******************************************************************** */
+/* WSM callback								*/
+void xradio_suspend_resume(struct xradio_vif *priv, struct wsm_suspend_resume *arg)
+{
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+
+#if 0
+	ap_printk(XRADIO_DBG_MSG, "[AP] %s: %s\n", 
+	          arg->stop ? "stop" : "start",
+	          arg->multicast ? "broadcast" : "unicast");
+#endif
+	if (arg->multicast) {
+		bool cancel_tmo = false;
+		spin_lock_bh(&priv->ps_state_lock);
+		if (arg->stop) {
+			priv->tx_multicast = false;
+		} else {
+			/* Firmware sends this indication every DTIM if there
+			 * is a STA in powersave connected. There is no reason
+			 * to suspend, following wakeup will consume much more
+			 * power than it could be saved. */
+			xradio_pm_stay_awake(&hw_priv->pm_state, (priv->join_dtim_period *
+			                     (priv->beacon_int + 20) * HZ / 1024));
+			priv->tx_multicast = priv->aid0_bit_set && priv->buffered_multicasts;
+			if (priv->tx_multicast) {
+				cancel_tmo = true;
+				xradio_bh_wakeup(hw_priv);
+			}
+		}
+		spin_unlock_bh(&priv->ps_state_lock);
+		if (cancel_tmo)
+			del_timer_sync(&priv->mcast_timeout);
+	} else {
+		spin_lock_bh(&priv->ps_state_lock);
+		xradio_ps_notify(priv, arg->link_id, arg->stop);
+		spin_unlock_bh(&priv->ps_state_lock);
+		if (!arg->stop)
+			xradio_bh_wakeup(hw_priv);
+	}
+	return;
+}
+
+/* ******************************************************************** */
+/* AP privates								*/
+
+static int xradio_upload_beacon(struct xradio_vif *priv)
+{
+	int ret = 0;
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	struct wsm_template_frame frame = {
+		.frame_type = WSM_FRAME_TYPE_BEACON,
+	};
+	struct ieee80211_mgmt *mgmt;
+	u8 *erp_inf, *ies, *ht_oper;
+	u32 ies_len;
+
+	if (priv->vif->p2p || hw_priv->channel->band == NL80211_BAND_5GHZ)
+		frame.rate = WSM_TRANSMIT_RATE_6;
+
+	frame.skb = ieee80211_beacon_get(priv->hw, priv->vif);
+	if (WARN_ON(!frame.skb))
+		return -ENOMEM;
+
+	mgmt = (void *)frame.skb->data;
+	ies  = mgmt->u.beacon.variable;
+	ies_len = frame.skb->len - (u32)(ies - (u8 *)mgmt);
+
+	ht_oper = (u8 *)cfg80211_find_ie( WLAN_EID_HT_OPERATION, ies, ies_len);
+	if (ht_oper) {
+		/* Enable RIFS*/
+		ht_oper[3] |= 8;
+	}
+
+	erp_inf = (u8 *)cfg80211_find_ie(WLAN_EID_ERP_INFO, ies, ies_len);
+	if (erp_inf) {
+		if (erp_inf[ERP_INFO_BYTE_OFFSET]
+				& WLAN_ERP_BARKER_PREAMBLE)
+			priv->erp_info |= WLAN_ERP_BARKER_PREAMBLE;
+		else
+			priv->erp_info &= ~WLAN_ERP_BARKER_PREAMBLE;
+
+		if (erp_inf[ERP_INFO_BYTE_OFFSET]
+				& WLAN_ERP_NON_ERP_PRESENT) {
+			priv->erp_info |= WLAN_ERP_USE_PROTECTION;
+			priv->erp_info |= WLAN_ERP_NON_ERP_PRESENT;
+		} else {
+			priv->erp_info &= ~WLAN_ERP_USE_PROTECTION;
+			priv->erp_info &= ~WLAN_ERP_NON_ERP_PRESENT;
+		}
+	}
+
+#ifdef HIDDEN_SSID
+	if (priv->hidden_ssid) {
+		u8 *ssid_ie;
+		u8 ssid_len;
+
+		ap_printk(XRADIO_DBG_NIY, "%s: hidden_ssid set\n", __func__);
+		ssid_ie = (u8 *)cfg80211_find_ie(WLAN_EID_SSID, ies, ies_len);
+		WARN_ON(!ssid_ie);
+		ssid_len = ssid_ie[1];
+		if (ssid_len) {
+			ap_printk(XRADIO_DBG_MSG, "hidden_ssid with zero content ssid\n");
+			ssid_ie[1] = 0;
+			memmove(ssid_ie + 2, ssid_ie + 2 + ssid_len,
+					(ies + ies_len -
+					 (ssid_ie + 2 + ssid_len)));
+			frame.skb->len -= ssid_len;
+		} else {
+			ap_printk(XRADIO_DBG_WARN, "hidden ssid with ssid len 0 not supported");
+			dev_kfree_skb(frame.skb);
+			return -1;
+		}
+	}
+#endif
+
+	ret = wsm_set_template_frame(hw_priv, &frame, priv->if_id);
+	if (!ret) {
+#ifdef PROBE_RESP_EXTRA_IE
+		ret = xradio_upload_proberesp(priv);
+#else
+		/* TODO: Distille probe resp; remove TIM
+		 * and other beacon-specific IEs */
+		*(__le16 *)frame.skb->data = __cpu_to_le16(
+		                             IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_PROBE_RESP);
+		frame.frame_type = WSM_FRAME_TYPE_PROBE_RESPONSE;
+		/* TODO: Ideally probe response template should separately
+		   configured by supplicant through openmac. This is a
+		   temporary work-around known to fail p2p group info
+		   attribute related tests
+		   */
+		if (0 /* priv->vif->p2p */)
+			ret = wsm_set_probe_responder(priv, true);
+		else {
+			ret = wsm_set_template_frame(hw_priv, &frame, priv->if_id);
+			WARN_ON(wsm_set_probe_responder(priv, false));
+		}
+#endif
+	}
+	dev_kfree_skb(frame.skb);
+
+	return ret;
+}
+
+#ifdef PROBE_RESP_EXTRA_IE
+static int xradio_upload_proberesp(struct xradio_vif *priv)
+{
+	int ret = 0;
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	struct wsm_template_frame frame = {
+		.frame_type = WSM_FRAME_TYPE_PROBE_RESPONSE,
+	};
+#ifdef HIDDEN_SSID
+	u8 *ssid_ie;
+#endif
+
+	if (priv->vif->p2p || hw_priv->channel->band == NL80211_BAND_5GHZ)
+		frame.rate = WSM_TRANSMIT_RATE_6;
+
+	frame.skb = ieee80211_proberesp_get(priv->hw, priv->vif);
+	if (WARN_ON(!frame.skb))
+		return -ENOMEM;
+
+#ifdef HIDDEN_SSID
+	if (priv->hidden_ssid) {
+		int offset;
+		u8 ssid_len;
+		/* we are assuming beacon from upper layer will always contain
+		   zero filled ssid for hidden ap. The beacon shall never have
+		   ssid len = 0.
+		  */
+
+		offset  = offsetof(struct ieee80211_mgmt, u.probe_resp.variable);
+		ssid_ie = (u8 *)cfg80211_find_ie(WLAN_EID_SSID, frame.skb->data + offset,
+		                           frame.skb->len - offset);
+		ssid_len = ssid_ie[1];
+		if (ssid_len && (ssid_len == priv->ssid_length)) {
+			memcpy(ssid_ie + 2, priv->ssid, ssid_len);
+		} else {
+			ap_printk(XRADIO_DBG_ERROR, "%s: hidden ssid with mismatched ssid_len %d\n",
+			         __func__, ssid_len);
+			dev_kfree_skb(frame.skb);
+			return -1;
+		}
+	}
+#endif
+	ret = wsm_set_template_frame(hw_priv, &frame,  priv->if_id);
+	WARN_ON(wsm_set_probe_responder(priv, false));
+
+	dev_kfree_skb(frame.skb);
+
+	return ret;
+}
+#endif
+
+static int xradio_upload_pspoll(struct xradio_vif *priv)
+{
+	int ret = 0;
+	struct wsm_template_frame frame = {
+		.frame_type = WSM_FRAME_TYPE_PS_POLL,
+		.rate = 0xFF,
+	};
+
+	frame.skb = ieee80211_pspoll_get(priv->hw, priv->vif);
+	if (WARN_ON(!frame.skb))
+		return -ENOMEM;
+	ret = wsm_set_template_frame(xrwl_vifpriv_to_hwpriv(priv), &frame, priv->if_id);
+	dev_kfree_skb(frame.skb);
+	return ret;
+}
+
+static int xradio_upload_null(struct xradio_vif *priv)
+{
+	int ret = 0;
+	struct wsm_template_frame frame = {
+		.frame_type = WSM_FRAME_TYPE_NULL,
+		.rate = 0xFF,
+	};
+
+	frame.skb = ieee80211_nullfunc_get(priv->hw, priv->vif, false);
+	if (WARN_ON(!frame.skb))
+		return -ENOMEM;
+
+	ret = wsm_set_template_frame(xrwl_vifpriv_to_hwpriv(priv), &frame, priv->if_id);
+	dev_kfree_skb(frame.skb);
+	return ret;
+}
+
+static int xradio_upload_qosnull(struct xradio_vif *priv)
+{
+	struct ieee80211_qos_hdr* qos_null_template;
+	struct sk_buff* skb;
+	int ret = 0;
+	struct xradio_common *hw_priv =xrwl_vifpriv_to_hwpriv(priv);
+	struct wsm_template_frame frame = {
+		.frame_type = WSM_FRAME_TYPE_QOS_NULL,
+		.rate = 0xFF,
+	};
+	if (!hw_priv) 
+		ap_printk(XRADIO_DBG_ERROR,"%s: Cannot find xradio_common pointer!\n",__FUNCTION__);
+	/*set qos template*/
+	skb = dev_alloc_skb(hw_priv->hw->extra_tx_headroom + sizeof(struct ieee80211_qos_hdr));
+	if (!skb) {
+		ap_printk(XRADIO_DBG_ERROR,"%s: failed to allocate buffer for qos  nullfunc template!\n",__FUNCTION__);
+		return -1;
+	}
+	skb_reserve(skb, hw_priv->hw->extra_tx_headroom);
+	qos_null_template = (struct ieee80211_qos_hdr *)skb_put(skb,sizeof(struct ieee80211_qos_hdr));
+	memset(qos_null_template, 0, sizeof(struct ieee80211_qos_hdr));
+	memcpy(qos_null_template->addr1, priv->vif->bss_conf.bssid, ETH_ALEN);
+	memcpy(qos_null_template->addr2, priv->vif->addr, ETH_ALEN);
+	memcpy(qos_null_template->addr3, priv->vif->bss_conf.bssid, ETH_ALEN);
+	qos_null_template->frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA |
+					     IEEE80211_STYPE_QOS_NULLFUNC |
+					     IEEE80211_FCTL_TODS);
+	frame.skb = skb;
+	ret = wsm_set_template_frame(hw_priv, &frame, priv->if_id);
+	dev_kfree_skb(frame.skb);
+	return ret;
+}
+
+/* This API is nolonegr present in WSC */
+#if 0
+static int xradio_enable_beaconing(struct xradio_vif *priv,
+				   bool enable)
+{
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	struct wsm_beacon_transmit transmit = {
+		.enableBeaconing = enable,
+	};
+
+	return wsm_beacon_transmit(hw_priv, &transmit, priv->if_id);
+}
+#endif
+
+static int xradio_start_ap(struct xradio_vif *priv)
+{
+	int ret;
+#ifndef HIDDEN_SSID
+	const u8 *ssidie;
+	struct sk_buff *skb;
+	int offset;
+#endif
+	struct ieee80211_bss_conf *conf = &priv->vif->bss_conf;
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	struct wsm_start start = {
+		.mode = priv->vif->p2p ? WSM_START_MODE_P2P_GO : WSM_START_MODE_AP,
+		/* TODO:COMBO:Change once mac80211 support is available */
+		.band = (hw_priv->channel->band == NL80211_BAND_5GHZ) ?
+				     WSM_PHY_BAND_5G : WSM_PHY_BAND_2_4G,
+		.channelNumber = hw_priv->channel->hw_value,
+		.beaconInterval = conf->beacon_int,
+		.DTIMPeriod = conf->dtim_period,
+		.preambleType = conf->use_short_preamble ?
+		                WSM_JOIN_PREAMBLE_SHORT :WSM_JOIN_PREAMBLE_LONG,
+		.probeDelay = 100,
+		.basicRateSet = xradio_rate_mask_to_wsm(hw_priv, conf->basic_rates),
+	};
+
+#ifdef TES_P2P_000B_EXTEND_INACTIVITY_CNT
+	///w, TES_P2P_000B WorkAround:
+	///w, when inactivity count of a peer device is zero,
+	///w, which will reset while receiving a peer device frame,
+	///w, firmware will disconnect with it.
+	///w, due to some reason, such as scan/phy error, we miss these frame.
+	///w, then we can't keep connection with peer device.
+	///w, we set the min_inactivity value to large as WorkAround.
+	//min_inactivity be modified to 20, yangfh.
+	struct wsm_inactivity inactivity = {
+		.min_inactivity = 20,
+		.max_inactivity = 10,
+	};
+#else
+	struct wsm_inactivity inactivity = {
+		.min_inactivity = 9,
+		.max_inactivity = 1,
+	};
+#endif
+
+	ap_printk(XRADIO_DBG_TRC,"%s\n", __FUNCTION__);
+
+	if (priv->if_id)
+		start.mode |= WSM_FLAG_MAC_INSTANCE_1;
+	else
+		start.mode &= ~WSM_FLAG_MAC_INSTANCE_1;
+
+	hw_priv->connected_sta_cnt = 0;
+
+#ifndef HIDDEN_SSID
+	/* Get SSID */
+	skb = ieee80211_beacon_get(priv->hw, priv->vif);
+	if (WARN_ON(!skb)) {
+		ap_printk(XRADIO_DBG_ERROR,"%s, ieee80211_beacon_get failed\n", __func__);
+		return -ENOMEM;
+	}
+
+	offset = offsetof(struct ieee80211_mgmt, u.beacon.variable);
+	ssidie = cfg80211_find_ie(WLAN_EID_SSID, skb->data + offset, skb->len - offset);
+
+	memset(priv->ssid, 0, sizeof(priv->ssid));
+	if (ssidie) {
+		priv->ssid_length = ssidie[1];
+		if (WARN_ON(priv->ssid_length > sizeof(priv->ssid)))
+			priv->ssid_length = sizeof(priv->ssid);
+		memcpy(priv->ssid, &ssidie[2], priv->ssid_length);
+	} else {
+		priv->ssid_length = 0;
+	}
+	dev_kfree_skb(skb);
+#endif
+
+	priv->beacon_int = conf->beacon_int;
+	priv->join_dtim_period = conf->dtim_period;
+	memset(&priv->last_tim[0], 0, sizeof(priv->last_tim)); //yangfh
+
+	start.ssidLength = priv->ssid_length;
+	memcpy(&start.ssid[0], priv->ssid, start.ssidLength);
+
+	memset(&priv->link_id_db, 0, sizeof(priv->link_id_db));
+
+	ap_printk(XRADIO_DBG_NIY, "[AP] ch: %d(%d), bcn: %d(%d),"
+	          "bss_rate: 0x%.8X, ssid: %.*s.\n",
+	          start.channelNumber,  start.band,
+	          start.beaconInterval, start.DTIMPeriod, 
+	          start.basicRateSet, start.ssidLength, start.ssid);
+	ret = WARN_ON(wsm_start(hw_priv, &start, priv->if_id));
+
+	if (!ret && priv->vif->p2p) {
+		ap_printk(XRADIO_DBG_NIY,"[AP] Setting p2p powersave configuration.\n");
+		WARN_ON(wsm_set_p2p_ps_modeinfo(hw_priv,
+			&priv->p2p_ps_modeinfo, priv->if_id));
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+		//xradio_notify_noa(priv, XRADIO_NOA_NOTIFICATION_DELAY);
+#endif
+	}
+
+	/*Set Inactivity time*/
+	if(!(strstr(&start.ssid[0], "6.1.12"))) {
+		wsm_set_inactivity(hw_priv, &inactivity, priv->if_id);
+	}
+	if (!ret) {
+#ifndef AP_AGGREGATE_FW_FIX
+		WARN_ON(wsm_set_block_ack_policy(hw_priv,
+		         XRADIO_TX_BLOCK_ACK_DISABLED_FOR_ALL_TID,
+		         XRADIO_RX_BLOCK_ACK_DISABLED_FOR_ALL_TID, priv->if_id));
+#else
+		if ((priv->if_id ==1) && !hw_priv->is_go_thru_go_neg)
+			WARN_ON(wsm_set_block_ack_policy(hw_priv,
+			         XRADIO_TX_BLOCK_ACK_ENABLED_FOR_ALL_TID, //modified for WFD by yangfh
+			         XRADIO_RX_BLOCK_ACK_ENABLED_FOR_ALL_TID, priv->if_id));
+		else
+			WARN_ON(wsm_set_block_ack_policy(hw_priv,
+			         XRADIO_TX_BLOCK_ACK_ENABLED_FOR_ALL_TID,
+			         XRADIO_RX_BLOCK_ACK_ENABLED_FOR_ALL_TID, priv->if_id));
+#endif
+		priv->join_status = XRADIO_JOIN_STATUS_AP;
+		/* xradio_update_filtering(priv); */
+	}
+	WARN_ON(wsm_set_operational_mode(hw_priv, &defaultoperationalmode, priv->if_id));
+	hw_priv->vif0_throttle = XRWL_HOST_VIF0_11BG_THROTTLE;
+	hw_priv->vif1_throttle = XRWL_HOST_VIF1_11BG_THROTTLE;
+	ap_printk(XRADIO_DBG_WARN, "vif%d, AP/GO mode THROTTLE=%d\n", priv->if_id,
+	          priv->if_id==0?hw_priv->vif0_throttle:hw_priv->vif1_throttle);
+	return ret;
+}
+
+static int xradio_update_beaconing(struct xradio_vif *priv)
+{
+	struct ieee80211_bss_conf *conf = &priv->vif->bss_conf;
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	struct wsm_reset reset = {
+		.link_id = 0,
+		.reset_statistics = true,
+	};
+	ap_printk(XRADIO_DBG_TRC,"%s\n", __FUNCTION__);
+
+	if (priv->mode == NL80211_IFTYPE_AP) {
+		/* TODO: check if changed channel, band */
+		if (priv->join_status != XRADIO_JOIN_STATUS_AP ||
+		    priv->beacon_int  != conf->beacon_int) {
+			ap_printk(XRADIO_DBG_WARN, "ap restarting!\n");
+			wsm_lock_tx(hw_priv);
+			if (priv->join_status != XRADIO_JOIN_STATUS_PASSIVE)
+				WARN_ON(wsm_reset(hw_priv, &reset, priv->if_id));
+			priv->join_status = XRADIO_JOIN_STATUS_PASSIVE;
+			WARN_ON(xradio_start_ap(priv));
+			wsm_unlock_tx(hw_priv);
+		} else
+			ap_printk(XRADIO_DBG_NIY, "ap started join_status: %d\n", priv->join_status);
+	}
+	return 0;
+}
+
+int xradio_find_link_id(struct xradio_vif *priv, const u8 *mac)
+{
+	int i, ret = 0;
+	spin_lock_bh(&priv->ps_state_lock);
+	for (i = 0; i < MAX_STA_IN_AP_MODE; ++i) {
+		if (!memcmp(mac, priv->link_id_db[i].mac, ETH_ALEN) &&
+			  priv->link_id_db[i].status) {
+			priv->link_id_db[i].timestamp = jiffies;
+			ret = i + 1;
+			break;
+		}
+	}
+	spin_unlock_bh(&priv->ps_state_lock);
+	return ret;
+}
+
+int xradio_alloc_link_id(struct xradio_vif *priv, const u8 *mac)
+{
+	int i, ret = 0;
+	unsigned long max_inactivity = 0;
+	unsigned long now = jiffies;
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+
+	spin_lock_bh(&priv->ps_state_lock);
+	for (i = 0; i < MAX_STA_IN_AP_MODE; ++i) {
+		if (!priv->link_id_db[i].status) {
+			ret = i + 1;
+			break;
+		} else if (priv->link_id_db[i].status != XRADIO_LINK_HARD &&
+		           !hw_priv->tx_queue_stats.link_map_cache[i + 1]) {
+			unsigned long inactivity = now - priv->link_id_db[i].timestamp;
+			if (inactivity < max_inactivity)
+				continue;
+			max_inactivity = inactivity;
+			ret = i + 1;
+		}
+	}
+	if (ret) {
+		struct xradio_link_entry *entry = &priv->link_id_db[ret - 1];
+		ap_printk(XRADIO_DBG_NIY, "STA added, link_id: %d\n", ret);
+		entry->status = XRADIO_LINK_RESERVE;
+		memcpy(&entry->mac, mac, ETH_ALEN);
+		memset(&entry->buffered, 0, XRADIO_MAX_TID);
+		skb_queue_head_init(&entry->rx_queue);
+		wsm_lock_tx_async(hw_priv);
+		if (queue_work(hw_priv->workqueue, &priv->link_id_work) <= 0)
+			wsm_unlock_tx(hw_priv);
+	} else {
+		ap_printk(XRADIO_DBG_WARN, "Early: no more link IDs available.\n");
+	}
+
+	spin_unlock_bh(&priv->ps_state_lock);
+	return ret;
+}
+
+void xradio_link_id_work(struct work_struct *work)
+{
+	struct xradio_vif *priv = container_of(work, struct xradio_vif, link_id_work);
+	struct xradio_common *hw_priv = priv->hw_priv;
+
+	wsm_flush_tx(hw_priv);
+	xradio_link_id_gc_work(&priv->link_id_gc_work.work);
+	wsm_unlock_tx(hw_priv);
+}
+
+void xradio_link_id_gc_work(struct work_struct *work)
+{
+	struct xradio_vif *priv =
+		container_of(work, struct xradio_vif, link_id_gc_work.work);
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	struct wsm_map_link map_link = {
+		.link_id = 0,
+	};
+	unsigned long now = jiffies;
+	unsigned long next_gc = -1;
+	long ttl;
+	bool need_reset;
+	u32 mask;
+	int i;
+
+	if (priv->join_status != XRADIO_JOIN_STATUS_AP)
+		return;
+
+	wsm_lock_tx(hw_priv);
+	spin_lock_bh(&priv->ps_state_lock);
+	for (i = 0; i < MAX_STA_IN_AP_MODE; ++i) {
+		need_reset = false;
+		mask = BIT(i + 1);
+		if (priv->link_id_db[i].status == XRADIO_LINK_RESERVE ||
+			  (priv->link_id_db[i].status == XRADIO_LINK_HARD && 
+			   !(priv->link_id_map & mask))) {
+			if (priv->link_id_map & mask) {
+				priv->sta_asleep_mask &= ~mask;
+				priv->pspoll_mask &= ~mask;
+				need_reset = true;
+			}
+			priv->link_id_map |= mask;
+			if (priv->link_id_db[i].status != XRADIO_LINK_HARD)
+				priv->link_id_db[i].status = XRADIO_LINK_SOFT;
+			memcpy(map_link.mac_addr, priv->link_id_db[i].mac, ETH_ALEN);
+			spin_unlock_bh(&priv->ps_state_lock);
+			if (need_reset) {
+				WARN_ON(xrwl_unmap_link(priv, i + 1));
+			}
+			map_link.link_id = i + 1;
+			WARN_ON(wsm_map_link(hw_priv, &map_link, priv->if_id));
+			next_gc = min(next_gc, XRADIO_LINK_ID_GC_TIMEOUT);
+			spin_lock_bh(&priv->ps_state_lock);
+		} else if (priv->link_id_db[i].status == XRADIO_LINK_SOFT) {
+			ttl = priv->link_id_db[i].timestamp - now + XRADIO_LINK_ID_GC_TIMEOUT;
+			if (ttl <= 0) {
+				need_reset = true;
+				priv->link_id_db[i].status = XRADIO_LINK_OFF;
+				priv->link_id_map &= ~mask;
+				priv->sta_asleep_mask &= ~mask;
+				priv->pspoll_mask &= ~mask;
+				memset(map_link.mac_addr, 0, ETH_ALEN);
+				spin_unlock_bh(&priv->ps_state_lock);
+				WARN_ON(xrwl_unmap_link(priv, i + 1));
+				spin_lock_bh(&priv->ps_state_lock);
+			} else {
+				next_gc = min_t(unsigned long, next_gc, ttl);
+			}
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+		} else if (priv->link_id_db[i].status == XRADIO_LINK_RESET ||
+		           priv->link_id_db[i].status == XRADIO_LINK_RESET_REMAP) {
+			int status = priv->link_id_db[i].status;
+			priv->link_id_db[i].status = XRADIO_LINK_OFF;
+			priv->link_id_db[i].timestamp = now;
+			spin_unlock_bh(&priv->ps_state_lock);
+			WARN_ON(xrwl_unmap_link(priv, i + 1));
+			if (status == XRADIO_LINK_RESET_REMAP) {
+				memcpy(map_link.mac_addr, priv->link_id_db[i].mac, ETH_ALEN);
+				map_link.link_id = i + 1;
+				WARN_ON(wsm_map_link(hw_priv, &map_link, priv->if_id));
+				next_gc = min(next_gc, XRADIO_LINK_ID_GC_TIMEOUT);
+				priv->link_id_db[i].status = priv->link_id_db[i].prev_status;
+			}
+			spin_lock_bh(&priv->ps_state_lock);
+#endif
+		}
+		if (need_reset) {
+			skb_queue_purge(&priv->link_id_db[i].rx_queue);
+			ap_printk(XRADIO_DBG_NIY, "STA removed, link_id: %d\n", i + 1);
+		}
+	}
+	spin_unlock_bh(&priv->ps_state_lock);
+	if (next_gc != -1)
+		queue_delayed_work(hw_priv->workqueue, &priv->link_id_gc_work, next_gc);
+	wsm_unlock_tx(hw_priv);
+}
+
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+#if 0
+void xradio_notify_noa(struct xradio_vif *priv, int delay)
+{
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	struct cfg80211_p2p_ps p2p_ps = {0};
+	struct wsm_p2p_ps_modeinfo *modeinfo;
+	modeinfo = &priv->p2p_ps_modeinfo;
+
+	if (priv->join_status != XRADIO_JOIN_STATUS_AP)
+		return;
+
+	if (delay)
+		msleep(delay);
+
+	if (!WARN_ON(wsm_get_p2p_ps_modeinfo(hw_priv, modeinfo))) {
+#if defined(CONFIG_XRADIO_DEBUG)
+		print_hex_dump_bytes("[AP] p2p_get_ps_modeinfo: ", DUMP_PREFIX_NONE,
+		                    (u8 *)modeinfo, sizeof(*modeinfo));
+#endif /* CONFIG_XRADIO_DEBUG */
+		p2p_ps.opp_ps = !!(modeinfo->oppPsCTWindow & BIT(7));
+		p2p_ps.ctwindow = modeinfo->oppPsCTWindow & (~BIT(7));
+		p2p_ps.count = modeinfo->count;
+		p2p_ps.start = __le32_to_cpu(modeinfo->startTime);
+		p2p_ps.duration = __le32_to_cpu(modeinfo->duration);
+		p2p_ps.interval = __le32_to_cpu(modeinfo->interval);
+		p2p_ps.index = modeinfo->reserved;
+
+		ieee80211_p2p_noa_notify(priv->vif, &p2p_ps, GFP_KERNEL);
+	}
+}
+#endif
+#endif
+int xrwl_unmap_link(struct xradio_vif *priv, int link_id)
+{
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	int ret = 0;
+
+	if (is_hardware_xradio(hw_priv)) {
+		struct wsm_map_link maplink = {
+			.link_id = link_id,
+			.unmap = true,
+		};
+		if (link_id)
+			memcpy(&maplink.mac_addr[0], priv->link_id_db[link_id - 1].mac, ETH_ALEN);
+		return wsm_map_link(hw_priv, &maplink, priv->if_id);
+	} else {
+		struct wsm_reset reset = {
+			.link_id = link_id,
+			.reset_statistics = true,
+		};
+		ret = wsm_reset(hw_priv, &reset, priv->if_id);
+		WARN_ON(wsm_set_operational_mode(hw_priv, &defaultoperationalmode, priv->if_id));
+		return ret;
+	}
+}
+#ifdef AP_HT_CAP_UPDATE
+void xradio_ht_oper_update_work(struct work_struct *work)
+{
+	struct sk_buff *skb;
+	struct ieee80211_mgmt *mgmt;
+	u8 *ht_oper, *ies;
+	u32 ies_len;
+	struct xradio_vif *priv =
+	        container_of(work, struct xradio_vif, ht_oper_update_work);
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	struct wsm_update_ie update_ie = {
+		.what = WSM_UPDATE_IE_BEACON,
+		.count = 1,
+	};
+
+	skb = ieee80211_beacon_get(priv->hw, priv->vif);
+	if (WARN_ON(!skb))
+		return;
+
+	mgmt = (void *)skb->data;
+	ies = mgmt->u.beacon.variable;
+	ies_len = skb->len - (u32)(ies - (u8 *)mgmt);
+	ht_oper= (u8 *)cfg80211_find_ie( WLAN_EID_HT_OPERATION, ies, ies_len);
+	if(ht_oper && priv->ht_oper == HT_INFO_MASK) {
+		ht_oper[HT_INFO_OFFSET] |= 0x11;
+		update_ie.ies = ht_oper;
+		update_ie.length = HT_INFO_IE_LEN;
+		WARN_ON(wsm_update_ie(hw_priv, &update_ie, priv->if_id));
+	}
+	dev_kfree_skb(skb);
+}
+#endif
diff --git a/drivers/net/wireless/xradio/ap.h b/drivers/net/wireless/xradio/ap.h
new file mode 100644
index 0000000..9d55fb8
--- /dev/null
+++ b/drivers/net/wireless/xradio/ap.h
@@ -0,0 +1,63 @@
+/*
+ * STA and AP APIs for XRadio drivers
+ *
+ * Copyright (c) 2013, XRadio
+ * Author: XRadio
+ *
+ * This program is free software; you can 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 AP_H_INCLUDED
+#define AP_H_INCLUDED
+
+#define XRADIO_NOA_NOTIFICATION_DELAY 10
+
+#ifdef AP_HT_CAP_UPDATE
+#define HT_INFO_OFFSET 4
+#define HT_INFO_MASK 0x0011
+#define HT_INFO_IE_LEN 22
+#endif
+
+int xradio_set_tim(struct ieee80211_hw *dev, struct ieee80211_sta *sta,
+		   bool set);
+int xradio_sta_add(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		   struct ieee80211_sta *sta);
+int xradio_sta_remove(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+		      struct ieee80211_sta *sta);
+void xradio_sta_notify(struct ieee80211_hw *dev, struct ieee80211_vif *vif,
+		       enum sta_notify_cmd notify_cmd,
+		       struct ieee80211_sta *sta);
+void xradio_bss_info_changed(struct ieee80211_hw *dev,
+			     struct ieee80211_vif *vif,
+			     struct ieee80211_bss_conf *info,
+			     u32 changed);
+int xradio_ampdu_action(struct ieee80211_hw *hw,
+			struct ieee80211_vif *vif,
+			struct ieee80211_ampdu_params *params);
+/*			enum ieee80211_ampdu_mlme_action action,
+			struct ieee80211_sta *sta, u16 tid, u16 *ssn,
+			u8 buf_size);*/
+
+void xradio_suspend_resume(struct xradio_vif *priv,
+			  struct wsm_suspend_resume *arg);
+void xradio_set_tim_work(struct work_struct *work);
+void xradio_set_cts_work(struct work_struct *work);
+void xradio_multicast_start_work(struct work_struct *work);
+void xradio_multicast_stop_work(struct work_struct *work);
+void xradio_mcast_timeout(struct timer_list *t);
+int xradio_find_link_id(struct xradio_vif *priv, const u8 *mac);
+int xradio_alloc_link_id(struct xradio_vif *priv, const u8 *mac);
+void xradio_link_id_work(struct work_struct *work);
+void xradio_link_id_gc_work(struct work_struct *work);
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+/*in linux3.4 mac,it does't have the noa pass*/
+//void xradio_notify_noa(struct xradio_vif *priv, int delay);
+#endif
+int xrwl_unmap_link(struct xradio_vif *priv, int link_id);
+#ifdef AP_HT_CAP_UPDATE
+void xradio_ht_oper_update_work(struct work_struct *work);
+#endif
+
+#endif
diff --git a/drivers/net/wireless/xradio/bh.c b/drivers/net/wireless/xradio/bh.c
new file mode 100644
index 0000000..5b53778
--- /dev/null
+++ b/drivers/net/wireless/xradio/bh.c
@@ -0,0 +1,836 @@
+/*
+ * Data Transmission thread implementation for XRadio drivers
+ *
+ * Copyright (c) 2013, XRadio
+ * Author: XRadio
+ *
+ * This program is free software; you can 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 <net/mac80211.h>
+#include <linux/kthread.h>
+
+#include "xradio.h"
+#include "bh.h"
+#include "hwio.h"
+#include "wsm.h"
+#include "sdio.h"
+
+/* TODO: Verify these numbers with WSM specification. */
+#define DOWNLOAD_BLOCK_SIZE_WR	(0x1000 - 4)
+/* an SPI message cannot be bigger than (2"12-1)*2 bytes
+ * "*2" to cvt to bytes */
+#define MAX_SZ_RD_WR_BUFFERS	(DOWNLOAD_BLOCK_SIZE_WR*2)
+#define PIGGYBACK_CTRL_REG	(2)
+#define EFFECTIVE_BUF_SIZE	(MAX_SZ_RD_WR_BUFFERS - PIGGYBACK_CTRL_REG)
+
+/* Suspend state privates */
+enum xradio_bh_pm_state {
+	XRADIO_BH_RESUMED = 0,
+	XRADIO_BH_SUSPEND,
+	XRADIO_BH_SUSPENDED,
+	XRADIO_BH_RESUME,
+};
+typedef int (*xradio_wsm_handler)(struct xradio_common *hw_priv, u8 *data, size_t size);
+
+#ifdef MCAST_FWDING
+int wsm_release_buffer_to_fw(struct xradio_vif *priv, int count);
+#endif
+static int xradio_bh(void *arg);
+
+int xradio_register_bh(struct xradio_common *hw_priv)
+{
+	int ret = 0;
+
+	atomic_set(&hw_priv->bh_tx, 0);
+	atomic_set(&hw_priv->bh_term, 0);
+	atomic_set(&hw_priv->bh_suspend, XRADIO_BH_RESUMED);
+	hw_priv->buf_id_tx = 0;
+	hw_priv->buf_id_rx = 0;
+	init_waitqueue_head(&hw_priv->bh_wq);
+	init_waitqueue_head(&hw_priv->bh_evt_wq);
+
+	hw_priv->bh_thread = kthread_run(&xradio_bh, hw_priv, XRADIO_BH_THREAD);
+	if (IS_ERR(hw_priv->bh_thread)) {
+		ret = PTR_ERR(hw_priv->bh_thread);
+		hw_priv->bh_thread = NULL;
+	}
+
+	return ret;
+}
+
+void xradio_unregister_bh(struct xradio_common *hw_priv)
+{
+	struct task_struct *thread = hw_priv->bh_thread;
+
+	if (WARN_ON(!thread))
+		return;
+
+	hw_priv->bh_thread = NULL;
+	kthread_stop(thread);
+#ifdef HAS_PUT_TASK_STRUCT
+	put_task_struct(thread);
+#endif
+	dev_dbg(hw_priv->pdev, "Unregister success.\n");
+}
+
+void xradio_irq_handler(struct xradio_common *hw_priv)
+{
+	xradio_bh_wakeup(hw_priv);
+}
+
+void xradio_bh_wakeup(struct xradio_common *hw_priv)
+{
+	atomic_set(&hw_priv->bh_tx, 1);
+	wake_up(&hw_priv->bh_wq);
+}
+
+int xradio_bh_suspend(struct xradio_common *hw_priv)
+{
+#ifdef MCAST_FWDING
+	int i =0;
+	struct xradio_vif *priv = NULL;
+#endif
+
+	if (hw_priv->bh_error) {
+		return -EINVAL;
+	}
+
+#ifdef MCAST_FWDING
+ 	xradio_for_each_vif(hw_priv, priv, i) {
+		if (!priv)
+			continue;	
+		if ( (priv->multicast_filter.enable)
+			&& (priv->join_status == XRADIO_JOIN_STATUS_AP) ) {
+			wsm_release_buffer_to_fw(priv,
+				(hw_priv->wsm_caps.numInpChBufs - 1));
+			break;
+		}
+	}
+#endif
+
+	atomic_set(&hw_priv->bh_suspend, XRADIO_BH_SUSPEND);
+	wake_up(&hw_priv->bh_wq);
+	return wait_event_timeout(hw_priv->bh_evt_wq, (hw_priv->bh_error || 
+	                          XRADIO_BH_SUSPENDED == atomic_read(&hw_priv->bh_suspend)),
+	                          1 * HZ)?  0 : -ETIMEDOUT;
+}
+
+int xradio_bh_resume(struct xradio_common *hw_priv)
+{
+#ifdef MCAST_FWDING
+	int ret;
+	int i =0; 
+	struct xradio_vif *priv =NULL;
+#endif
+
+
+	if (hw_priv->bh_error) {
+		return -EINVAL;
+	}
+
+	atomic_set(&hw_priv->bh_suspend, XRADIO_BH_RESUME);
+	wake_up(&hw_priv->bh_wq);
+
+#ifdef MCAST_FWDING
+	ret = wait_event_timeout(hw_priv->bh_evt_wq, (hw_priv->bh_error ||
+	                         XRADIO_BH_RESUMED == atomic_read(&hw_priv->bh_suspend))
+	                         ,1 * HZ)? 0 : -ETIMEDOUT;
+
+	xradio_for_each_vif(hw_priv, priv, i) {
+		if (!priv)
+			continue;
+		if ((priv->join_status == XRADIO_JOIN_STATUS_AP) && 
+			  (priv->multicast_filter.enable)) {
+			u8 count = 0;
+			WARN_ON(wsm_request_buffer_request(priv, &count));
+			dev_dbg(hw_priv->pdev, "Reclaim Buff %d \n",count);
+			break;
+		}
+	}
+
+	return ret;
+#else
+	return wait_event_timeout(hw_priv->bh_evt_wq,hw_priv->bh_error ||
+		(XRADIO_BH_RESUMED == atomic_read(&hw_priv->bh_suspend)),
+		1 * HZ) ? 0 : -ETIMEDOUT;
+#endif
+
+}
+
+static inline void wsm_alloc_tx_buffer(struct xradio_common *hw_priv)
+{
+	++hw_priv->hw_bufs_used;
+}
+
+int wsm_release_tx_buffer(struct xradio_common *hw_priv, int count)
+{
+	int ret = 0;
+	int hw_bufs_used = hw_priv->hw_bufs_used;
+
+	hw_priv->hw_bufs_used -= count;
+	if (WARN_ON(hw_priv->hw_bufs_used < 0)) {
+		/* Tx data patch stops when all but one hw buffers are used.
+		   So, re-start tx path in case we find hw_bufs_used equals
+		   numInputChBufs - 1.
+		*/
+		dev_err(hw_priv->pdev, "hw_bufs_used=%d, count=%d.\n",
+				hw_priv->hw_bufs_used, count);
+		ret = -1;
+	} else if (hw_bufs_used >= (hw_priv->wsm_caps.numInpChBufs - 1))
+		ret = 1;
+	if (!hw_priv->hw_bufs_used)
+		wake_up(&hw_priv->bh_evt_wq);
+	return ret;
+}
+
+int wsm_release_vif_tx_buffer(struct xradio_common *hw_priv, int if_id, int count)
+{
+	int ret = 0;
+
+	hw_priv->hw_bufs_used_vif[if_id] -= count;
+	if (!hw_priv->hw_bufs_used_vif[if_id])
+		wake_up(&hw_priv->bh_evt_wq);
+
+	if (WARN_ON(hw_priv->hw_bufs_used_vif[if_id] < 0))
+		ret = -1;
+	return ret;
+}
+#ifdef MCAST_FWDING
+int wsm_release_buffer_to_fw(struct xradio_vif *priv, int count)
+{
+	int i;
+	u8 flags;
+	struct wsm_buf *buf;
+	size_t buf_len;
+	struct wsm_hdr *wsm;
+	struct xradio_common *hw_priv = priv->hw_priv;
+
+	if (priv->join_status != XRADIO_JOIN_STATUS_AP) {
+		return 0;
+	}
+	dev_dbg(hw_priv->pdev, "Rel buffer to FW %d, %d\n", count, hw_priv->hw_bufs_used);
+
+	for (i = 0; i < count; i++) {
+		if ((hw_priv->hw_bufs_used + 1) < hw_priv->wsm_caps.numInpChBufs) {
+			flags = i ? 0: 0x1;
+
+			wsm_alloc_tx_buffer(hw_priv);
+			buf = &hw_priv->wsm_release_buf[i];
+			buf_len = buf->data - buf->begin;
+
+			/* Add sequence number */
+			wsm = (struct wsm_hdr *)buf->begin;
+			BUG_ON(buf_len < sizeof(*wsm));
+
+			wsm->id &= __cpu_to_le32(~WSM_TX_SEQ(WSM_TX_SEQ_MAX));
+			wsm->id |= cpu_to_le32(WSM_TX_SEQ(hw_priv->wsm_tx_seq));
+
+			dev_dbg(hw_priv->pdev, "REL %d\n", hw_priv->wsm_tx_seq);
+			if (WARN_ON(xradio_data_write(hw_priv, buf->begin, buf_len))) {
+				break;
+			}
+			hw_priv->buf_released = 1;
+			hw_priv->wsm_tx_seq = (hw_priv->wsm_tx_seq + 1) & WSM_TX_SEQ_MAX;
+		} else
+			break;
+	}
+
+	if (i == count) {
+		return 0;
+	}
+
+	/* Should not be here */
+	dev_dbg(hw_priv->pdev, "Error, Less HW buf %d,%d.\n",
+	          hw_priv->hw_bufs_used, hw_priv->wsm_caps.numInpChBufs);
+	WARN_ON(1);
+	return -1;
+}
+#endif
+
+/* reserve a packet for the case dev_alloc_skb failed in bh.*/
+int xradio_init_resv_skb(struct xradio_common *hw_priv)
+{
+	int len = (SDIO_BLOCK_SIZE<<2) + WSM_TX_EXTRA_HEADROOM + \
+	           8 + 12;	/* TKIP IV + ICV and MIC */
+
+	hw_priv->skb_reserved = dev_alloc_skb(len);
+	if (hw_priv->skb_reserved) {
+		hw_priv->skb_resv_len = len;
+	} else {
+		dev_warn(hw_priv->pdev, "xr_alloc_skb failed(%d)\n", len);
+	}
+	return 0;
+}
+
+void xradio_deinit_resv_skb(struct xradio_common *hw_priv)
+{
+	if (hw_priv->skb_reserved) {
+		dev_kfree_skb(hw_priv->skb_reserved);
+		hw_priv->skb_reserved = NULL;
+		hw_priv->skb_resv_len = 0;
+	}
+}
+
+int xradio_realloc_resv_skb(struct xradio_common *hw_priv,
+							struct sk_buff *skb)
+{
+	if (!hw_priv->skb_reserved && hw_priv->skb_resv_len) {
+		hw_priv->skb_reserved = dev_alloc_skb(hw_priv->skb_resv_len);
+		if (!hw_priv->skb_reserved) {
+			hw_priv->skb_reserved = skb;
+			dev_warn(hw_priv->pdev, "xr_alloc_skb failed(%d)\n",
+					hw_priv->skb_resv_len);
+			return -1;
+		}
+	}
+	return 0; /* realloc sbk success, deliver to upper.*/
+}
+
+static inline struct sk_buff *xradio_get_resv_skb(struct xradio_common *hw_priv,
+												  size_t len)
+{	struct sk_buff *skb = NULL;
+	if (hw_priv->skb_reserved && len <= hw_priv->skb_resv_len) {
+		skb = hw_priv->skb_reserved;
+		hw_priv->skb_reserved = NULL;
+	}
+	return skb;
+}
+
+static inline int xradio_put_resv_skb(struct xradio_common *hw_priv,
+									  struct sk_buff *skb)
+{
+	if (!hw_priv->skb_reserved && hw_priv->skb_resv_len) {
+		hw_priv->skb_reserved = skb;
+		return 0;
+	}
+	return 1; /* sbk not put to reserve*/
+}
+
+static struct sk_buff *xradio_get_skb(struct xradio_common *hw_priv, size_t len)
+{
+	struct sk_buff *skb = NULL;
+	size_t alloc_len = (len > SDIO_BLOCK_SIZE) ? len : SDIO_BLOCK_SIZE;
+
+	/* TKIP IV + TKIP ICV and MIC - Piggyback.*/
+	alloc_len += WSM_TX_EXTRA_HEADROOM + 8 + 12- 2;
+	if (len > SDIO_BLOCK_SIZE || !hw_priv->skb_cache) {
+		skb = dev_alloc_skb(alloc_len);
+		/* In AP mode RXed SKB can be looped back as a broadcast.
+		 * Here we reserve enough space for headers. */
+		if (skb) {
+			skb_reserve(skb, WSM_TX_EXTRA_HEADROOM + 8 /* TKIP IV */
+					    - WSM_RX_EXTRA_HEADROOM);
+		} else {
+			skb = xradio_get_resv_skb(hw_priv, alloc_len);
+			if (skb) {
+				dev_dbg(hw_priv->pdev, "get skb_reserved(%d)!\n", alloc_len);
+				skb_reserve(skb, WSM_TX_EXTRA_HEADROOM + 8 /* TKIP IV */
+						    - WSM_RX_EXTRA_HEADROOM);
+			} else {
+				dev_dbg(hw_priv->pdev, "xr_alloc_skb failed(%d)!\n", alloc_len);
+			}
+		}
+	} else {
+		skb = hw_priv->skb_cache;
+		hw_priv->skb_cache = NULL;
+	}
+	return skb;
+}
+
+static void xradio_put_skb(struct xradio_common *hw_priv, struct sk_buff *skb)
+{
+	if (hw_priv->skb_cache)
+		dev_kfree_skb(skb);
+	else
+		hw_priv->skb_cache = skb;
+}
+
+static int xradio_bh_read_ctrl_reg(struct xradio_common *hw_priv,
+					  u16 *ctrl_reg)
+{
+	int ret;
+	ret = xradio_reg_read_16(hw_priv, HIF_CONTROL_REG_ID, ctrl_reg);
+	if (ret) {
+		ret = xradio_reg_read_16(hw_priv, HIF_CONTROL_REG_ID, ctrl_reg);
+		if (ret) {
+			hw_priv->bh_error = 1;
+			dev_err(hw_priv->pdev, "Failed to read control register.\n");
+		}
+	}
+
+	return ret;
+}
+
+static int xradio_device_wakeup(struct xradio_common *hw_priv)
+{
+	u16 ctrl_reg;
+	int ret, i=0;
+
+	/* To force the device to be always-on, the host sets WLAN_UP to 1 */
+	ret = xradio_reg_write_16(hw_priv, HIF_CONTROL_REG_ID, HIF_CTRL_WUP_BIT);
+	if (WARN_ON(ret))
+		return ret;
+
+	ret = xradio_bh_read_ctrl_reg(hw_priv, &ctrl_reg);
+	if (WARN_ON(ret))
+		return ret;
+
+	/* If the device returns WLAN_RDY as 1, the device is active and will
+	 * remain active. */
+	while (!(ctrl_reg & HIF_CTRL_RDY_BIT) && i < 500) {
+		ret = xradio_bh_read_ctrl_reg(hw_priv, &ctrl_reg);
+		msleep(1);
+		i++;
+	}
+	if (unlikely(i >= 500)) {
+		dev_err(hw_priv->pdev, "Device cannot wakeup.\n");
+		return -1;
+	} else if (unlikely(i >= 50))
+		dev_warn(hw_priv->pdev, "Device wakeup time=%dms.\n", i);
+	dev_dbg(hw_priv->pdev, "Device awake, t=%dms.\n", i);
+	return 1;
+}
+
+/* Must be called from BH thraed. */
+void xradio_enable_powersave(struct xradio_vif *priv,
+			     bool enable)
+{
+	priv->powersave_enabled = enable;
+}
+
+static void xradio_bh_rx_dump(struct device *dev, u8 *data, size_t len){
+#ifdef DEBUG
+	static const char *msgnames[0xffff] = {
+			// 0x4?? is a sync response to a command
+			[0x0404] = "tx confirm",
+			[0x0406] = "mib confirm",
+			[0x0407] = "scan started",
+			[0x0409] = "configuration confirm",
+			[0x040a] = "reset confirm",
+			[0x040b] = "join confirm",
+			[0x040c] = "key added",
+			[0x040d] = "key removed",
+			[0x0410] = "pm confirm",
+			[0x0411] = "set bss params",
+			[0x0412] = "tx queue params",
+			[0x0413] = "edca confirm",
+			[0x0417] = "start confirm",
+			[0x041b] = "update ie confirm",
+			[0x041c] = "map link confirm",
+			// 0x8?? seem to be async responses or events
+			[0x0801] = "firmware startup complete",
+			[0x0804] = "rx",
+			[0x0805] = "event",
+			[0x0806] = "scan complete",
+			[0x0810] = "set pm indication"
+	};
+
+	u16 msgid, ifid;
+	u16 *p = (u16 *)data;
+	msgid = (*(p + 1)) & 0xC3F;
+	ifid  = (*(p + 1)) >> 6;
+	ifid &= 0xF;
+	const char *msgname = msgnames[msgid];
+	if(msgid == 0x804 && ifid == 2){
+		msgname = "scan result";
+	}
+
+	dev_dbg(dev, "vif %d: sdio rx, msgid %s(0x%.4X) len %d\n",
+			ifid, msgname, msgid, *p);
+//	print_hex_dump_bytes("<-- ", DUMP_PREFIX_NONE,
+//	                     data, min(len, (size_t) 64));
+#endif
+}
+
+#define READLEN(ctrl) ((ctrl & HIF_CTRL_NEXT_LEN_MASK) << 1) //read_len=ctrl_reg*2.
+
+static int xradio_bh_rx_availlen(struct xradio_common *hw_priv){
+	u16 ctrl_reg = 0;
+	if (xradio_bh_read_ctrl_reg(hw_priv, &ctrl_reg)) {
+		return -EIO;
+	}
+	return READLEN(ctrl_reg);
+}
+
+static int xradio_bh_rx(struct xradio_common *hw_priv, u16* nextlen) {
+	size_t read_len = 0;
+	struct sk_buff *skb_rx = NULL;
+	struct wsm_hdr *wsm;
+	size_t wsm_len;
+	int wsm_id;
+	u8 wsm_seq;
+	size_t alloc_len;
+	u8 *data;
+	int ret;
+
+	read_len = *nextlen > 0 ? *nextlen : xradio_bh_rx_availlen(hw_priv);
+	if(read_len <= 0)
+		return read_len;
+
+	if (read_len < sizeof(struct wsm_hdr) || (read_len > EFFECTIVE_BUF_SIZE)) {
+		dev_err(hw_priv->pdev, "Invalid read len: %d", read_len);
+		return -1;
+	}
+
+	/* Add SIZE of PIGGYBACK reg (CONTROL Reg)
+	 * to the NEXT Message length + 2 Bytes for SKB */
+	read_len = read_len + 2;
+
+	alloc_len = sdio_align_len(hw_priv, read_len);
+	/* Check if not exceeding XRADIO capabilities */
+	if (WARN_ON_ONCE(alloc_len > EFFECTIVE_BUF_SIZE)) {
+		dev_err(hw_priv->pdev, "Read aligned len: %d\n", alloc_len);
+	}
+
+	/* Get skb buffer. */
+	skb_rx = xradio_get_skb(hw_priv, alloc_len);
+	if (!skb_rx) {
+		dev_err(hw_priv->pdev, "xradio_get_skb failed.\n");
+		return -ENOMEM;
+	}
+	skb_trim(skb_rx, 0);
+	skb_put(skb_rx, read_len);
+	data = skb_rx->data;
+	if (!data) {
+		dev_err(hw_priv->pdev, "skb data is NULL.\n");
+		ret = -ENOMEM;
+		goto out;
+	}
+
+	/* Read data from device. */
+	if (xradio_data_read(hw_priv, data, alloc_len)) {
+		ret = -EIO;
+		goto out;
+	}
+
+	/* the ctrl register is appened to the end of the wsm frame
+	 * so we can use this to avoid reading the control register
+	 * again for the next read .. but if this is invalid we'll
+	 * do an invalid read and the firmware will crash so this
+	 * probably needs some sort of validation */
+	*nextlen = READLEN(__le16_to_cpu(((__le16 *) data)[(alloc_len >> 1) - 1]));
+
+	/* check wsm length. */
+	wsm = (struct wsm_hdr *) data;
+	wsm_len = __le32_to_cpu(wsm->len);
+
+	if (WARN_ON(wsm_len > read_len)) {
+		dev_err(hw_priv->pdev, "wsm is bigger than data read, read %d but frame is %d\n",
+				read_len, wsm_len);
+		ret = -1;
+		goto out;
+	}
+
+	/* dump rx data. */
+	xradio_bh_rx_dump(hw_priv->pdev, data, wsm_len);
+
+	/* extract wsm id and seq. */
+	wsm_id = __le32_to_cpu(wsm->id) & 0xFFF;
+	wsm_seq = (__le32_to_cpu(wsm->id) >> 13) & 7;
+	skb_trim(skb_rx, wsm_len);
+
+	/* process exceptions. */
+	if (wsm_id == 0) {
+		printk("wtf?\n");
+		ret = 0;
+		goto out;
+	} else if (unlikely(wsm_id == 0x0800)) {
+		dev_err(hw_priv->pdev, "firmware exception!\n");
+		wsm_handle_exception(hw_priv, &data[sizeof(*wsm)],
+				wsm_len - sizeof(*wsm));
+		ret = -1;
+		goto out;
+	}
+
+	hw_priv->wsm_rx_seq = (wsm_seq + 1) & 7;
+
+	/* Process tx frames confirm. */
+	if (wsm_id & 0x0400) {
+		if (wsm_release_tx_buffer(hw_priv, 1) < 0) {
+			dev_err(hw_priv->pdev, "tx buffer < 0.\n");
+			ret  = -1;
+			goto out;
+		}
+	}
+
+	/* WSM processing frames. */
+	if (wsm_handle_rx(hw_priv, wsm_id, wsm, &skb_rx)) {
+		dev_err(hw_priv->pdev, "wsm_handle_rx failed.\n");
+		ret = -1;
+		goto out;
+	}
+
+	ret = 1;
+
+out:
+	/* Reclaim the SKB buffer */
+	if (skb_rx) {
+		if (xradio_put_resv_skb(hw_priv, skb_rx))
+			xradio_put_skb(hw_priv, skb_rx);
+	}
+
+	return ret;
+}
+
+static void xradio_bh_tx_dump(struct device *dev, u8 *data, size_t len){
+#ifdef DEBUG
+	static const char *msgnames[0xffff] = {
+			[0x0004] = "tx",
+			[0x0006] = "MIB",
+			[0x0007] = "start scan",
+			[0x0009] = "configure",
+			[0x000A] = "reset",
+			[0x000B] = "join",
+			[0x000C] = "add key",
+			[0x000D] = "remove key",
+			[0x0010] = "set pm",
+			[0x0011] = "set bss params",
+			[0x0012] = "set tx queue params",
+			[0x0013] = "set edca",
+			[0x0017] = "start",
+			[0x001b] = "update ie",
+			[0x001c] = "map link",
+	};
+	static const char *mibnames[0xffff] = {
+			[0x0003] = "DOT11_SLOT_TIME",
+			[0x1002] = "TEMPLATE_FRAME",
+			[0x1003] = "RX_FILTER",
+			[0x1004] = "BEACON_FILTER_TABLE",
+			[0x1005] = "BEACON_FILTER_ENABLE",
+			[0x1006] = "OPERATIONAL POWER MODE",
+			[0x1007] = "BEACON_WAKEUP_PERIOD",
+			[0x1009] = "RCPI_RSSI_THRESHOLD",
+			[0x1010] = "SET_ASSOCIATION_MODE",
+			[0x100e] = "BLOCK_ACK_POLICY",
+			[0x100f] = "OVERRIDE_INTERNAL_TX_RATE",
+			[0x1013] = "SET_UAPSD_INFORMATION",
+			[0x1016] = "SET_TX_RATE_RETRY_POLICY",
+			[0x1020] = "PROTECTED_MGMT_POLICY",
+			[0x1021] = "SET_HT_PROTECTION",
+			[0x1024] = "USE_MULTI_TX_CONF",
+			[0x1025] = "KEEP_ALIVE_PERIOD",
+			[0x1026] = "DISABLE_BSSID_FILTER",
+			[0x1035] = "SET_INACTIVITY",
+	};
+
+	u16 msgid, ifid, mib;
+	u16 *p = (u16 *)data;
+	msgid = (*(p + 1)) & 0x3F;
+	ifid  = (*(p + 1)) >> 6;
+	ifid &= 0xF;
+	mib = *(p + 2);
+
+	WARN_ON(msgnames[msgid] == NULL);
+
+	if (msgid == 0x0006) {
+		dev_dbg(dev, "vif %d: sdio tx, msgid %s(0x%.4X) len %d MIB %s(0x%.4X)\n",
+				ifid, msgnames[msgid], msgid,*p, mibnames[mib], mib);
+	} else {
+		dev_dbg(dev, "vif %d: sdio tx, msgid %s(0x%.4X) len %d\n", ifid, msgnames[msgid], msgid, *p);
+	}
+
+//	print_hex_dump_bytes("--> ", DUMP_PREFIX_NONE, data,
+//	                     min(len, (size_t) 64));
+#endif
+}
+
+static int xradio_bh_tx(struct xradio_common *hw_priv){
+	int txavailable;
+	int txburst;
+	int vif_selected;
+	struct wsm_hdr *wsm;
+	size_t tx_len;
+	int ret;
+	u8 *data;
+
+	BUG_ON(hw_priv->hw_bufs_used > hw_priv->wsm_caps.numInpChBufs);
+	txavailable = hw_priv->wsm_caps.numInpChBufs - hw_priv->hw_bufs_used;
+	if (txavailable) {
+		/* Wake up the devices */
+		if (hw_priv->device_can_sleep) {
+			ret = xradio_device_wakeup(hw_priv);
+			if (WARN_ON(ret < 0)) {
+				return -1;
+			} else if (ret) {
+				hw_priv->device_can_sleep = false;
+			} else { /* Wait for "awake" interrupt */
+				dev_dbg(hw_priv->pdev,
+						"need to wait for device to wake before doing tx\n");
+				return 0;
+			}
+		}
+		/* Increase Tx buffer*/
+		wsm_alloc_tx_buffer(hw_priv);
+
+		/* Get data to send and send it. */
+		ret = wsm_get_tx(hw_priv, &data, &tx_len, &txburst, &vif_selected);
+		if (ret <= 0) {
+			wsm_release_tx_buffer(hw_priv, 1);
+			if (WARN_ON(ret < 0)) {
+				dev_err(hw_priv->pdev, "wsm_get_tx=%d.\n", ret);
+				return -ENOMEM;
+			} else {
+				return 0;
+			}
+		} else {
+			wsm = (struct wsm_hdr *) data;
+			BUG_ON(tx_len < sizeof(*wsm));
+			BUG_ON(__le32_to_cpu(wsm->len) != tx_len);
+
+			/* Align tx length and check it. */
+			if (tx_len <= 8)
+			tx_len = 16;
+			tx_len = sdio_align_len(hw_priv, tx_len);
+
+			/* Check if not exceeding XRADIO capabilities */
+			if (tx_len > EFFECTIVE_BUF_SIZE) {
+				dev_warn(hw_priv->pdev, "Write aligned len: %d\n", tx_len);
+			}
+
+			/* Make sequence number. */
+			wsm->id &= __cpu_to_le32(~WSM_TX_SEQ(WSM_TX_SEQ_MAX));
+			wsm->id |= cpu_to_le32(WSM_TX_SEQ(hw_priv->wsm_tx_seq));
+
+			/* Send the data to devices. */
+			if (WARN_ON(xradio_data_write(hw_priv, data, tx_len))) {
+				wsm_release_tx_buffer(hw_priv, 1);
+				dev_err(hw_priv->pdev, "xradio_data_write failed\n");
+				return -EIO;
+			}
+
+			xradio_bh_tx_dump(hw_priv->pdev, data, tx_len);
+
+			/* Process after data have sent. */
+			if (vif_selected != -1) {
+				hw_priv->hw_bufs_used_vif[vif_selected]++;
+			}
+			wsm_txed(hw_priv, data);
+			hw_priv->wsm_tx_seq = (hw_priv->wsm_tx_seq + 1) & WSM_TX_SEQ_MAX;
+
+			return 1;
+		}
+	} else
+		return 0;
+}
+
+static int xradio_bh_exchange(struct xradio_common *hw_priv) {
+	int rxdone;
+	int txdone;
+	u16 nextlen = 0;
+
+	/* query stuck frames in firmware. */
+	if (atomic_xchg(&hw_priv->query_cnt, 0)) {
+		if (schedule_work(&hw_priv->query_work) <= 0)
+			atomic_add(1, &hw_priv->query_cnt);
+	}
+
+	/* keep doing tx and rx until they both stop or we are told
+	 * to terminate */
+	do {
+		txdone = xradio_bh_tx(hw_priv);
+		if (txdone < 0) {
+			break;
+		}
+		rxdone = xradio_bh_rx(hw_priv, &nextlen);
+		if (rxdone < 0) {
+			break;
+		}
+	} while ((txdone > 0 || rxdone > 0) && !kthread_should_stop());
+	return 0;
+}
+
+static int xradio_bh(void *arg)
+{
+	struct xradio_common *hw_priv = arg;
+	int term, suspend;
+	int wake = 0;
+	long timeout;
+	long status;
+
+	for (;;) {
+		timeout = HZ / 30;
+
+		// wait for something to happen or a timeout
+		status = wait_event_interruptible_timeout(hw_priv->bh_wq, ( {
+					wake = atomic_xchg(&hw_priv->bh_tx, 0);
+					term = kthread_should_stop();
+					suspend = atomic_read(&hw_priv->bh_suspend);
+					(wake || term || suspend);}), timeout);
+
+		if (wake) {
+			if(xradio_bh_exchange(hw_priv) < 0){
+				break;
+			}
+		} else if (term) {
+			dev_dbg(hw_priv->pdev, "xradio_bh exit!\n");
+			break;
+		} else if (status < 0) {
+			dev_err(hw_priv->pdev, "bh_error=%d, status=%ld\n",
+					hw_priv->bh_error, status);
+			break;
+		} else if (!status) {
+			/* check if there is data waiting but we missed the interrupt*/
+			if (xradio_bh_rx_availlen(hw_priv) > 0) {
+				dev_warn(hw_priv->pdev, "missed interrupt\n");
+				if(xradio_bh_exchange(hw_priv) < 0){
+					break;
+				}
+			}
+			/* There are some frames to be confirmed. */
+			else if (hw_priv->hw_bufs_used) {
+				long timeout = 0;
+				bool pending = 0;
+				dev_dbg(hw_priv->pdev, "Need confirm:%d!\n",
+						hw_priv->hw_bufs_used);
+				/* Check if frame transmission is timed out. */
+				pending = xradio_query_txpkt_timeout(hw_priv, XRWL_ALL_IFS,
+						hw_priv->pending_frame_id, &timeout);
+				/* There are some frames confirm time out. */
+				if (pending && timeout < 0) {
+					dev_err(hw_priv->pdev, "query_txpkt_timeout:%ld!\n",
+							timeout);
+					break;
+				}
+			} //else if (!txpending){
+			  //if (hw_priv->powersave_enabled && !hw_priv->device_can_sleep && !atomic_read(&hw_priv->recent_scan)) {
+			  //	/* Device is idle, we can go to sleep. */
+			  //	dev_dbg(hw_priv->pdev, "Device idle(timeout), can sleep.\n");
+			  //	WARN_ON(xradio_reg_write_16(hw_priv, HIF_CONTROL_REG_ID, 0));
+			  //	hw_priv->device_can_sleep = true;
+			  //}
+			  //continue;
+			  //}
+		} else if (suspend) {
+			dev_dbg(hw_priv->pdev, "Host suspend request.\n");
+			/* Check powersave setting again. */
+			if (hw_priv->powersave_enabled) {
+				dev_dbg(hw_priv->pdev,
+						"Device idle(host suspend), can sleep.\n");
+				WARN_ON(xradio_reg_write_16(hw_priv, HIF_CONTROL_REG_ID, 0));
+				hw_priv->device_can_sleep = true;
+			}
+
+			/* bh thread go to suspend. */
+			atomic_set(&hw_priv->bh_suspend, XRADIO_BH_SUSPENDED);
+			wake_up(&hw_priv->bh_evt_wq);
+			status = wait_event_interruptible(hw_priv->bh_wq,
+					XRADIO_BH_RESUME == atomic_read(&hw_priv->bh_suspend));
+
+			if (status < 0) {
+				dev_err(hw_priv->pdev, "Failed to wait for resume: %ld.\n",
+						status);
+				break;
+			}
+			dev_dbg(hw_priv->pdev, "Host resume.\n");
+			atomic_set(&hw_priv->bh_suspend, XRADIO_BH_RESUMED);
+			wake_up(&hw_priv->bh_evt_wq);
+		}
+	} /* for (;;)*/
+
+	dev_err(hw_priv->pdev, "bh thread exiting\n");
+
+	return 0;
+}
diff --git a/drivers/net/wireless/xradio/bh.h b/drivers/net/wireless/xradio/bh.h
new file mode 100644
index 0000000..ca9187e
--- /dev/null
+++ b/drivers/net/wireless/xradio/bh.h
@@ -0,0 +1,36 @@
+/*
+ * Data Transmission thread for XRadio drivers
+ *
+ * Copyright (c) 2013, XRadio
+ * Author: XRadio
+ *
+ * This program is free software; you can 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 XRADIO_BH_H
+#define XRADIO_BH_H
+
+#define XRADIO_BH_THREAD   "xradio_bh"
+
+/* extern */ struct xradio_common;
+
+#define SDIO_BLOCK_SIZE (528)
+
+int xradio_register_bh(struct xradio_common *hw_priv);
+void xradio_unregister_bh(struct xradio_common *hw_priv);
+void xradio_irq_handler(struct xradio_common *hw_priv);
+void xradio_bh_wakeup(struct xradio_common *hw_priv);
+int xradio_bh_suspend(struct xradio_common *hw_priv);
+int xradio_bh_resume(struct xradio_common *hw_priv);
+/* Must be called from BH thread. */
+void xradio_enable_powersave(struct xradio_vif *priv, bool enable);
+int wsm_release_tx_buffer(struct xradio_common *hw_priv, int count);
+int wsm_release_vif_tx_buffer(struct xradio_common *hw_priv, int if_id,
+                              int count);
+int xradio_init_resv_skb(struct xradio_common *hw_priv);
+void xradio_deinit_resv_skb(struct xradio_common *hw_priv);
+int xradio_realloc_resv_skb(struct xradio_common *hw_priv,
+							struct sk_buff *skb);
+#endif /* XRADIO_BH_H */
diff --git a/drivers/net/wireless/xradio/common.h b/drivers/net/wireless/xradio/common.h
new file mode 100644
index 0000000..1ce23de
--- /dev/null
+++ b/drivers/net/wireless/xradio/common.h
@@ -0,0 +1,101 @@
+/*
+ * Common interfaces for XRadio drivers
+ *
+ * Copyright (c) 2013, XRadio
+ * Author: XRadio
+ *
+ * This program is free software; you can 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 XRADIO_COMMON_H
+#define XRADIO_COMMON_H
+
+/*******************************************************
+ interfaces for parse frame protocol info.
+********************************************************/
+#define LLC_LEN       8
+#define LLC_TYPE_OFF  6  //Ether type offset
+#define IP_PROTO_OFF  9  //protocol offset
+#define IP_S_ADD_OFF  12
+#define IP_D_ADD_OFF  16
+#define UDP_LEN       8
+//DHCP
+#define DHCP_BOOTP_C  68
+#define DHCP_BOOTP_S  67
+#define UDP_BOOTP_LEN 236  //exclude "Options:64"
+#define BOOTP_OPS_LEN 64
+#define DHCP_MAGIC    0x63825363
+#define DHCP_DISCOVER 0x01
+#define DHCP_OFFER    0x02
+#define DHCP_REQUEST  0x03
+#define DHCP_DECLINE  0x04
+#define DHCP_ACK      0x05
+#define DHCP_NACK     0x06
+#define DHCP_RELEASE  0x07
+
+//LLC layer.
+static inline bool is_SNAP(u8* llc_data)
+{
+	return (bool)(*(u16*)(llc_data) == 0xAAAA && llc_data[2] == 0x03);  //0xAA, 0xAA, 0x03.
+}
+
+static inline bool is_STP(u8* llc_data)
+{
+	return (bool)(*(u16*)(llc_data) == 0xAAAA && llc_data[2] == 0x03);  //0x42, 0x42, 0x03.
+}
+
+//IP/IPV6/ARP layer...
+static inline bool is_ip(u8* llc_data)
+{
+	return (bool)(*(u16*)(llc_data+LLC_TYPE_OFF) == cpu_to_be16(ETH_P_IP));   //0x0800
+}
+static inline bool is_ipv6(u8* llc_data)
+{
+	return (bool)(*(u16*)(llc_data+LLC_TYPE_OFF) == cpu_to_be16(ETH_P_IPV6)); //0x08dd
+}
+static inline bool is_arp(u8* llc_data)
+{
+	return (bool)(*(u16*)(llc_data+LLC_TYPE_OFF) == cpu_to_be16(ETH_P_ARP));  //0x0806
+}
+static inline bool is_8021x(u8* llc_data)
+{
+	return (bool)(*(u16*)(llc_data+LLC_TYPE_OFF) == cpu_to_be16(ETH_P_PAE));  //0x888E
+}
+
+//TCP/UDP layer...
+static inline bool is_tcp(u8* llc_data)
+{
+	return (bool)(llc_data[LLC_LEN+IP_PROTO_OFF] == IPPROTO_TCP);  //
+}
+
+static inline bool is_udp(u8* llc_data)
+{
+	return (bool)(llc_data[LLC_LEN+IP_PROTO_OFF] == IPPROTO_UDP);  //
+}
+
+static inline bool is_icmp(u8* llc_data)
+{
+	return (bool)(llc_data[LLC_LEN+IP_PROTO_OFF] == IPPROTO_ICMP);  //
+}
+
+static inline bool is_igmp(u8* llc_data)
+{
+	return (bool)(llc_data[LLC_LEN+IP_PROTO_OFF] == IPPROTO_IGMP);  //
+}
+
+static inline bool is_dhcp(u8* llc_data)
+{
+	u8* ip_hdr  = llc_data+LLC_LEN;
+	if(!is_ip(llc_data))
+		return (bool)0;
+	if(ip_hdr[IP_PROTO_OFF] == IPPROTO_UDP) {
+		u8* udp_hdr = ip_hdr+((ip_hdr[0]&0xf)<<2);  //ihl:words
+		return (bool)((((udp_hdr[0]<<8)|udp_hdr[1]) == DHCP_BOOTP_C) ||  //DHCP client
+		              (((udp_hdr[0]<<8)|udp_hdr[1]) == DHCP_BOOTP_S));   //DHCP server
+	}
+	return (bool)0;
+}
+
+#endif //XRADIO_COMMON_H
diff --git a/drivers/net/wireless/xradio/debug.h b/drivers/net/wireless/xradio/debug.h
new file mode 100644
index 0000000..30a59f3
--- /dev/null
+++ b/drivers/net/wireless/xradio/debug.h
@@ -0,0 +1,22 @@
+/*
+ * DebugFS code for XRadio drivers
+ *
+ * Copyright (c) 2013, XRadio
+ * Author: XRadio
+ *
+ * This program is free software; you can 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 XRADIO_DEBUG_H_INCLUDED
+#define XRADIO_DEBUG_H_INCLUDED
+
+#define xradio_dbg(level, ...)
+#define txrx_printk(level, ...)
+#define wsm_printk(level, ...)
+#define sta_printk(level, ...)
+#define scan_printk(level, ...)
+#define ap_printk(level, ...)
+#define pm_printk(level, ...)
+
+#endif /* XRADIO_DEBUG_H_INCLUDED */
diff --git a/drivers/net/wireless/xradio/fwio.c b/drivers/net/wireless/xradio/fwio.c
new file mode 100644
index 0000000..cfb45eb
--- /dev/null
+++ b/drivers/net/wireless/xradio/fwio.c
@@ -0,0 +1,560 @@
+/*
+ * Firmware I/O implementation for XRadio drivers
+ *
+ * Copyright (c) 2013, XRadio
+ * Author: XRadio
+ *
+ * This program is free software; you can 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/vmalloc.h>
+#include <linux/sched.h>
+#include <linux/firmware.h>
+
+#include "xradio.h"
+#include "fwio.h"
+#include "hwio.h"
+#include "bh.h"
+#include "sdio.h"
+
+/* Macroses are local. */
+#define APB_WRITE(reg, val) \
+	do { \
+		ret = xradio_apb_write_32(hw_priv, APB_ADDR(reg), (val)); \
+		if (ret < 0) { \
+			dev_err(hw_priv->pdev, \
+				"%s: can't write %s at line %d.\n", \
+				__func__, #reg, __LINE__); \
+			goto error; \
+		} \
+	} while (0)
+#define APB_READ(reg, val) \
+	do { \
+		ret = xradio_apb_read_32(hw_priv, APB_ADDR(reg), &(val)); \
+		if (ret < 0) { \
+			dev_err(hw_priv->pdev, \
+				"%s: can't read %s at line %d.\n", \
+				__func__, #reg, __LINE__); \
+			goto error; \
+		} \
+	} while (0)
+#define REG_WRITE(reg, val) \
+	do { \
+		ret = xradio_reg_write_32(hw_priv, (reg), (val)); \
+		if (ret < 0) { \
+			dev_err(hw_priv->pdev, \
+				"%s: can't write %s at line %d.\n", \
+				__func__, #reg, __LINE__); \
+			goto error; \
+		} \
+	} while (0)
+#define REG_READ(reg, val) \
+	do { \
+		ret = xradio_reg_read_32(hw_priv, (reg), &(val)); \
+		if (ret < 0) { \
+			dev_err(hw_priv->pdev, \
+				"%s: can't read %s at line %d.\n", \
+				__func__, #reg, __LINE__); \
+			goto error; \
+		} \
+	} while (0)
+
+
+static int xradio_get_hw_type(u32 config_reg_val, int *major_revision)
+{
+	int hw_type  = -1;
+	u32 hif_type = (config_reg_val >> 24) & 0x4;
+	//u32 hif_vers = (config_reg_val >> 31) & 0x1;
+
+	/* Check if we have XRADIO*/
+  if (hif_type == 0x4) {
+		*major_revision = 0x4;
+		hw_type = HIF_HW_TYPE_XRADIO;
+	} else {
+		//hw type unknown.
+		*major_revision = 0x0;
+	}
+	return hw_type;
+}
+
+/*
+ * This function is called to Parse the SDD file
+ * to extract some informations
+ */
+static int xradio_parse_sdd(struct xradio_common *hw_priv, u32 *dpll)
+{
+	int ret = 0;
+	const char *sdd_path = NULL;
+	struct xradio_sdd *pElement = NULL;
+	int parsedLength = 0;
+
+	BUG_ON(hw_priv->sdd != NULL);
+
+	/* select and load sdd file depend on hardware version. */
+	switch (hw_priv->hw_revision) {
+	case XR819_HW_REV0:
+		sdd_path = XR819_SDD_FILE;
+		break;
+	default:
+		dev_dbg(hw_priv->pdev, "unknown hardware version.\n");
+		return ret;
+	}
+
+	ret = request_firmware(&hw_priv->sdd, sdd_path, hw_priv->pdev);
+	if (unlikely(ret)) {
+		dev_dbg(hw_priv->pdev, "can't load sdd file %s.\n",
+				sdd_path);
+		return ret;
+	}
+
+	//parse SDD config.
+	hw_priv->is_BT_Present = false;
+	pElement = (struct xradio_sdd *)hw_priv->sdd->data;
+	parsedLength += (FIELD_OFFSET(struct xradio_sdd, data) + pElement->length);
+	pElement = FIND_NEXT_ELT(pElement);
+
+	while (parsedLength < hw_priv->sdd->size) {
+		switch (pElement->id) {
+		case SDD_PTA_CFG_ELT_ID:
+			hw_priv->conf_listen_interval = (*((u16 *)pElement->data+1) >> 7) & 0x1F;
+			hw_priv->is_BT_Present = true;
+			xradio_dbg(XRADIO_DBG_NIY, "PTA element found.Listen Interval %d\n",
+			           hw_priv->conf_listen_interval);
+			break;
+		case SDD_REFERENCE_FREQUENCY_ELT_ID:
+			switch(*((uint16_t*)pElement->data)) {
+			case 0x32C8:
+				*dpll = 0x1D89D241;
+				break;
+			case 0x3E80:
+				*dpll = 0x1E1;
+				break;
+			case 0x41A0:
+				*dpll = 0x124931C1;
+				break;
+			case 0x4B00:
+				*dpll = 0x191;
+				break;
+			case 0x5DC0:
+				*dpll = 0x141;
+				break;
+			case 0x6590:
+				*dpll = 0x0EC4F121;
+				break;
+			case 0x8340:
+				*dpll = 0x92490E1;
+				break;
+			case 0x9600:
+				*dpll = 0x100010C1;
+				break;
+			case 0x9C40:
+				*dpll = 0xC1;
+				break;
+			case 0xBB80:
+				*dpll = 0xA1;
+				break;
+			case 0xCB20:
+				*dpll = 0x7627091;
+				break;
+			default:
+				*dpll = DPLL_INIT_VAL_XRADIO;
+				xradio_dbg(XRADIO_DBG_WARN, "Unknown Reference clock frequency." 
+				           "Use default DPLL value=0x%08x.", DPLL_INIT_VAL_XRADIO);
+				break;
+			}
+		default:
+			break;
+		}
+		parsedLength += (FIELD_OFFSET(struct xradio_sdd, data) + pElement->length);
+		pElement = FIND_NEXT_ELT(pElement);
+	}
+	
+	dev_dbg(hw_priv->pdev, "sdd size=%d parse len=%d.\n",
+	           hw_priv->sdd->size, parsedLength);
+
+	//
+	if (hw_priv->is_BT_Present == false) {
+		hw_priv->conf_listen_interval = 0;
+		xradio_dbg(XRADIO_DBG_NIY, "PTA element NOT found.\n");
+	}
+	return ret;
+}
+
+static int xradio_firmware(struct xradio_common *hw_priv)
+{
+	int ret, block, num_blocks;
+	unsigned i;
+	u32 val32;
+	u32 put = 0, get = 0;
+	u8 *buf = NULL;
+	const char *fw_path;
+	const struct firmware *firmware = NULL;
+
+	switch (hw_priv->hw_revision) {
+	case XR819_HW_REV0:
+		fw_path = XR819_FIRMWARE;
+		break;
+	default:
+		dev_dbg(hw_priv->pdev, "invalid silicon revision %d.\n",
+				hw_priv->hw_revision);
+		return -EINVAL;
+	}
+	/* Initialize common registers */
+	APB_WRITE(DOWNLOAD_IMAGE_SIZE_REG, DOWNLOAD_ARE_YOU_HERE);
+	APB_WRITE(DOWNLOAD_PUT_REG, 0);
+	APB_WRITE(DOWNLOAD_GET_REG, 0);
+	APB_WRITE(DOWNLOAD_STATUS_REG, DOWNLOAD_PENDING);
+	APB_WRITE(DOWNLOAD_FLAGS_REG, 0);
+
+	/* Release CPU from RESET */
+	REG_READ(HIF_CONFIG_REG_ID, val32);
+	val32 &= ~HIF_CONFIG_CPU_RESET_BIT;
+	REG_WRITE(HIF_CONFIG_REG_ID, val32);
+
+	/* Enable Clock */
+	val32 &= ~HIF_CONFIG_CPU_CLK_DIS_BIT;
+	REG_WRITE(HIF_CONFIG_REG_ID, val32);
+
+	/* Load a firmware file */
+	ret = request_firmware(&firmware, fw_path, hw_priv->pdev);
+	if (ret) {
+		dev_dbg(hw_priv->pdev, "can't load firmware file %s.\n",
+				fw_path);
+		goto error;
+	}
+	BUG_ON(!firmware->data);
+
+	buf = kmalloc(DOWNLOAD_BLOCK_SIZE, GFP_KERNEL);
+	if (!buf) {
+		dev_dbg(hw_priv->pdev, "can't allocate firmware buffer.\n");
+		ret = -ENOMEM;
+		goto error;
+	}
+
+	/* Check if the bootloader is ready */
+	for (i = 0; i < 100; i++/*= 1 + i / 2*/) {
+		APB_READ(DOWNLOAD_IMAGE_SIZE_REG, val32);
+		if (val32 == DOWNLOAD_I_AM_HERE)
+			break;
+		mdelay(10);
+	} /* End of for loop */
+	if (val32 != DOWNLOAD_I_AM_HERE) {
+		dev_dbg(hw_priv->pdev, "bootloader is not ready.\n");
+		ret = -ETIMEDOUT;
+		goto error;
+	}
+
+	/* Calculcate number of download blocks */
+	num_blocks = (firmware->size - 1) / DOWNLOAD_BLOCK_SIZE + 1;
+
+	/* Updating the length in Download Ctrl Area */
+	val32 = firmware->size; /* Explicit cast from size_t to u32 */
+	APB_WRITE(DOWNLOAD_IMAGE_SIZE_REG, val32);
+
+	/* Firmware downloading loop */
+	for (block = 0; block < num_blocks ; block++) {
+		size_t tx_size;
+		size_t block_size;
+
+		/* check the download status */
+		APB_READ(DOWNLOAD_STATUS_REG, val32);
+		if (val32 != DOWNLOAD_PENDING) {
+			dev_dbg(hw_priv->pdev, "bootloader reported error %d.\n",
+					val32);
+			ret = -EIO;
+			goto error;
+		}
+
+		/* loop until put - get <= 24K */
+		for (i = 0; i < 100; i++) {
+			APB_READ(DOWNLOAD_GET_REG, get);
+			if ((put - get) <= (DOWNLOAD_FIFO_SIZE - DOWNLOAD_BLOCK_SIZE))
+				break;
+			mdelay(i);
+		}
+
+		if ((put - get) > (DOWNLOAD_FIFO_SIZE - DOWNLOAD_BLOCK_SIZE)) {
+			dev_dbg(hw_priv->pdev, "Timeout waiting for FIFO.\n");
+			ret = -ETIMEDOUT;
+			goto error;
+		}
+
+		/* calculate the block size */
+		tx_size = block_size = min((size_t)(firmware->size - put), (size_t)DOWNLOAD_BLOCK_SIZE);
+		memcpy(buf, &firmware->data[put], block_size);
+
+		if (block_size < DOWNLOAD_BLOCK_SIZE) {
+			memset(&buf[block_size], 0, DOWNLOAD_BLOCK_SIZE - block_size);
+			tx_size = DOWNLOAD_BLOCK_SIZE;
+		}
+
+		/* send the block to sram */
+		ret = xradio_apb_write(hw_priv, APB_ADDR(DOWNLOAD_FIFO_OFFSET + (put & (DOWNLOAD_FIFO_SIZE - 1))), 
+		                       buf, tx_size);
+		if (ret < 0) {
+			dev_err(hw_priv->pdev, "%s: can't write block at line %d.\n", __func__, __LINE__);
+			goto error;
+		}
+
+		/* update the put register */
+		put += block_size;
+		APB_WRITE(DOWNLOAD_PUT_REG, put);
+	} /* End of firmware download loop */
+
+	/* Wait for the download completion */
+	for (i = 0; i < 300; i += 1 + i / 2) {
+		APB_READ(DOWNLOAD_STATUS_REG, val32);
+		if (val32 != DOWNLOAD_PENDING)
+			break;
+		mdelay(i);
+	}
+	if (val32 != DOWNLOAD_SUCCESS) {
+		dev_dbg(hw_priv->pdev, "wait for download completion failed. " \
+		           "Read: 0x%.8X\n", val32);
+		ret = -ETIMEDOUT;
+		goto error;
+	} else {
+		dev_dbg(hw_priv->pdev, "Firmware completed.\n");
+		ret = 0;
+	}
+
+error:
+	if(buf)
+		kfree(buf);
+	if (firmware) {
+		release_firmware(firmware);
+	}
+	return ret;
+}
+
+static int xradio_bootloader(struct xradio_common *hw_priv)
+{
+	const char *bl_path = XR819_BOOTLOADER;
+	u32  addr = AHB_MEMORY_ADDRESS;
+	int ret;
+	u32 i;
+	u32 *data;
+	const struct firmware *bootloader;
+
+	/* Load a bootloader file */
+	ret = request_firmware(&bootloader, bl_path, hw_priv->pdev);
+	if (ret) {
+		dev_dbg(hw_priv->pdev, "can't load bootloader file %s.\n",
+				bl_path);
+		goto error;
+	}
+
+	/* Down bootloader. */
+	data = (u32 *)bootloader->data;
+	for(i = 0; i < (bootloader->size)/4; i++, addr+=4) {
+		REG_WRITE(HIF_SRAM_BASE_ADDR_REG_ID, addr);
+		REG_WRITE(HIF_AHB_DPORT_REG_ID,data[i]);
+	}
+	dev_dbg(hw_priv->pdev, "Bootloader complete\n");
+
+error:
+	if(bootloader) {
+		release_firmware(bootloader);
+	}
+	return ret;  
+}
+
+bool test_retry = false;
+int xradio_load_firmware(struct xradio_common *hw_priv)
+{
+	int ret;
+	int i;
+	u32 val32;
+	u16 val16;
+	u32 dpll = 0;
+	int major_revision;
+
+	/* Read CONFIG Register Value - We will read 32 bits */
+	ret = xradio_reg_read_32(hw_priv, HIF_CONFIG_REG_ID, &val32);
+	if (ret < 0) {
+		dev_dbg(hw_priv->pdev, "can't read config register, err=%d.\n",
+				ret);
+		return ret;
+	}
+
+	//check hardware type and revision.
+	hw_priv->hw_type = xradio_get_hw_type(val32, &major_revision);
+	switch (hw_priv->hw_type) {
+	case HIF_HW_TYPE_XRADIO:
+		dev_dbg(hw_priv->pdev, "HW_TYPE_XRADIO detected.\n");
+		break;
+	default:
+		dev_dbg(hw_priv->pdev, "Unknown hardware: %d.\n",
+				hw_priv->hw_type);
+		return -ENOTSUPP;
+	}
+	if (major_revision == 4) {
+		hw_priv->hw_revision = XR819_HW_REV0;
+		dev_dbg(hw_priv->pdev, "XRADIO_HW_REV 1.0 detected.\n");
+	} else {
+		dev_dbg(hw_priv->pdev, "Unsupported major revision %d.\n",
+				major_revision);
+		return -ENOTSUPP;
+	}
+	
+	//load sdd file, and get config from it.
+	ret = xradio_parse_sdd(hw_priv, &dpll);
+	if (ret < 0) {
+		return ret;
+	}
+
+	//set dpll initial value and check.
+	ret = xradio_reg_write_32(hw_priv, HIF_TSET_GEN_R_W_REG_ID, dpll);
+	if (ret < 0) {
+		dev_dbg(hw_priv->pdev, "can't write DPLL register.\n");
+		goto out;
+	}
+	msleep(5);
+	ret = xradio_reg_read_32(hw_priv, HIF_TSET_GEN_R_W_REG_ID, &val32);
+	if (ret < 0) {
+		dev_dbg(hw_priv->pdev, "can't read DPLL register.\n");
+		goto out;
+	}
+	if (val32 != dpll) {
+		dev_dbg(hw_priv->pdev, "unable to initialise " \
+		           "DPLL register. Wrote 0x%.8X, read 0x%.8X.\n",
+				   dpll, val32);
+		ret = -EIO;
+		goto out;
+	}
+
+	/* Set wakeup bit in device */
+	ret = xradio_reg_read_16(hw_priv, HIF_CONTROL_REG_ID, &val16);
+	if (ret < 0) {
+		dev_dbg(hw_priv->pdev, "set_wakeup: can't read control register.\n");
+		goto out;
+	}
+	ret = xradio_reg_write_16(hw_priv, HIF_CONTROL_REG_ID, val16 | HIF_CTRL_WUP_BIT);
+	if (ret < 0) {
+		dev_dbg(hw_priv->pdev, "set_wakeup: can't write control register.\n");
+		goto out;
+	}
+
+	/* Wait for wakeup */
+	for (i = 0 ; i < 300 ; i += 1 + i / 2) {
+		ret = xradio_reg_read_16(hw_priv, HIF_CONTROL_REG_ID, &val16);
+		if (ret < 0) {
+			dev_dbg(hw_priv->pdev, "Wait_for_wakeup: "
+			           "can't read control register.\n");
+			goto out;
+		}
+		if (val16 & HIF_CTRL_RDY_BIT) {
+			break;
+		}
+		msleep(i);
+	}
+	if ((val16 & HIF_CTRL_RDY_BIT) == 0) {
+		dev_dbg(hw_priv->pdev, "Wait for wakeup:"
+		           "device is not responding.\n");
+		ret = -ETIMEDOUT;
+		goto out;
+	} else {
+		dev_dbg(hw_priv->pdev, "WLAN device is ready.\n");
+	}
+
+	/* Checking for access mode and download firmware. */
+	ret = xradio_reg_read_32(hw_priv, HIF_CONFIG_REG_ID, &val32);
+	if (ret < 0) {
+		dev_dbg(hw_priv->pdev, "check_access_mode: "
+		           "can't read config register.\n");
+		goto out;
+	}
+	if (val32 & HIF_CONFIG_ACCESS_MODE_BIT) {
+		/* Down bootloader. */
+		ret = xradio_bootloader(hw_priv);
+		if (ret < 0) {
+			dev_dbg(hw_priv->pdev, "can't download bootloader.\n");
+			goto out;
+		}
+		/* Down firmware. */
+		ret = xradio_firmware(hw_priv);
+		if (ret < 0) {
+			dev_dbg(hw_priv->pdev, "can't download firmware.\n");
+			goto out;
+		}
+	} else {
+		dev_dbg(hw_priv->pdev, "check_access_mode: "
+		           "device is already in QUEUE mode.\n");
+		/* TODO: verify this branch. Do we need something to do? */
+	}
+
+	if (HIF_HW_TYPE_XRADIO  == hw_priv->hw_type) {
+		/* If device is XRADIO the IRQ enable/disable bits
+		 * are in CONFIG register */
+		ret = xradio_reg_read_32(hw_priv, HIF_CONFIG_REG_ID, &val32);
+		if (ret < 0) {
+			dev_dbg(hw_priv->pdev, "enable_irq: can't read " \
+			           "config register.\n");
+			goto unsubscribe;
+		}
+		ret = xradio_reg_write_32(hw_priv, HIF_CONFIG_REG_ID,
+			val32 | HIF_CONF_IRQ_RDY_ENABLE);
+		if (ret < 0) {
+			dev_dbg(hw_priv->pdev, "enable_irq: can't write " \
+			           "config register.\n");
+			goto unsubscribe;
+		}
+	} else {
+		/* If device is XRADIO the IRQ enable/disable bits
+		 * are in CONTROL register */
+		/* Enable device interrupts - Both DATA_RDY and WLAN_RDY */
+		ret = xradio_reg_read_16(hw_priv, HIF_CONFIG_REG_ID, &val16);
+		if (ret < 0) {
+			dev_dbg(hw_priv->pdev, "enable_irq: can't read " \
+			           "control register.\n");
+			goto unsubscribe;
+		}
+		ret = xradio_reg_write_16(hw_priv, HIF_CONFIG_REG_ID, 
+		                          val16 | HIF_CTRL_IRQ_RDY_ENABLE);
+		if (ret < 0) {
+			dev_dbg(hw_priv->pdev, "enable_irq: can't write " \
+			           "control register.\n");
+			goto unsubscribe;
+		}
+
+	}
+
+	/* Configure device for MESSSAGE MODE */
+	ret = xradio_reg_read_32(hw_priv, HIF_CONFIG_REG_ID, &val32);
+	if (ret < 0) {
+		dev_dbg(hw_priv->pdev, "set_mode: can't read config register.\n");
+		goto unsubscribe;
+	}
+	ret = xradio_reg_write_32(hw_priv, HIF_CONFIG_REG_ID,
+	                          val32 & ~HIF_CONFIG_ACCESS_MODE_BIT);
+	if (ret < 0) {
+		dev_dbg(hw_priv->pdev, "set_mode: can't write config register.\n");
+		goto unsubscribe;
+	}
+
+	/* Unless we read the CONFIG Register we are
+	 * not able to get an interrupt */
+	mdelay(10);
+	xradio_reg_read_32(hw_priv, HIF_CONFIG_REG_ID, &val32);
+	return 0;
+
+unsubscribe:
+out:
+	if (hw_priv->sdd) {
+		release_firmware(hw_priv->sdd);
+		hw_priv->sdd = NULL;
+	}
+	return ret;
+}
+
+int xradio_dev_deinit(struct xradio_common *hw_priv)
+{
+	if (hw_priv->sdd) {
+		release_firmware(hw_priv->sdd);
+		hw_priv->sdd = NULL;
+	}
+	return 0;
+}
diff --git a/drivers/net/wireless/xradio/fwio.h b/drivers/net/wireless/xradio/fwio.h
new file mode 100644
index 0000000..b5efeb1
--- /dev/null
+++ b/drivers/net/wireless/xradio/fwio.h
@@ -0,0 +1,33 @@
+/*
+ * Firmware APIs for XRadio drivers
+ *
+ * Copyright (c) 2013, XRadio
+ * Author: XRadio
+ *
+ * This program is free software; you can 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 FWIO_H_INCLUDED
+#define FWIO_H_INCLUDED
+
+#define XR819_HW_REV0       (8190)
+#define XR819_BOOTLOADER    ("xr819/boot_xr819.bin")
+#define XR819_FIRMWARE      ("xr819/fw_xr819.bin")
+#define XR819_SDD_FILE      ("xr819/sdd_xr819.bin")
+
+#define SDD_PTA_CFG_ELT_ID             0xEB
+#define SDD_REFERENCE_FREQUENCY_ELT_ID 0xC5
+#define FIELD_OFFSET(type, field) ((u8 *)&((type *)0)->field - (u8 *)0)
+#define FIND_NEXT_ELT(e) (struct xradio_sdd *)((u8 *)&e->data + e->length)
+struct xradio_sdd {
+	u8 id;
+	u8 length;
+	u8 data[];
+};
+
+struct xradio_common;
+int xradio_load_firmware(struct xradio_common *hw_priv);
+int xradio_dev_deinit(struct xradio_common *hw_priv);
+
+#endif
diff --git a/drivers/net/wireless/xradio/ht.c b/drivers/net/wireless/xradio/ht.c
new file mode 100644
index 0000000..a9b3630
--- /dev/null
+++ b/drivers/net/wireless/xradio/ht.c
@@ -0,0 +1,86 @@
+#include <net/mac80211.h>
+
+#include "xradio.h"
+#include "sta.h"
+
+#define AG_RATE_INDEX  6     //11a/g rate for important short frames in 5G.
+
+#ifdef AP_HT_COMPAT_FIX
+#define AP_COMPAT_THRESHOLD  2000
+#define AP_COMPAT_MIN_CNT    200
+u8 ap_compat_bssid[ETH_ALEN] = {0};
+int xradio_apcompat_detect(struct xradio_vif *priv, u8 rx_rate)
+{
+	if (rx_rate < AG_RATE_INDEX) {
+		priv->ht_compat_cnt++;
+		txrx_printk(XRADIO_DBG_MSG,"%s:rate=%d.\n", __func__, rx_rate);
+	} else {
+		priv->ht_compat_det |= 1;
+		priv->ht_compat_cnt = 0;
+		txrx_printk(XRADIO_DBG_NIY,"%s:HT compat detect\n", __func__);
+		return 0;
+	}
+
+	/* Enhance compatibility with some illegal APs.*/
+	if (priv->ht_compat_cnt  > AP_COMPAT_THRESHOLD ||
+		(priv->ht_compat_cnt > AP_COMPAT_MIN_CNT &&
+		 priv->bssid[0] == 0xC8 &&
+		 priv->bssid[1] == 0x3A &&
+		 priv->bssid[2] == 0x35)) {
+		struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+		memcpy(ap_compat_bssid, priv->bssid, ETH_ALEN);
+		wms_send_disassoc_to_self(hw_priv, priv);
+		txrx_printk(XRADIO_DBG_WARN, "%s:SSID=%s, BSSID=" \
+		            "%02x:%02x:%02x:%02x:%02x:%02x\n", __func__, priv->ssid,
+		            ap_compat_bssid[0], ap_compat_bssid[1],
+		            ap_compat_bssid[2], ap_compat_bssid[3],
+		            ap_compat_bssid[4], ap_compat_bssid[5]);
+		return 1;
+	}
+	return 0;
+}
+
+void xradio_remove_ht_ie(struct xradio_vif *priv, struct sk_buff *skb)
+{
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
+	u8 *ies        = NULL;
+	size_t ies_len = 0;
+	u8 *ht_ie      = NULL;
+
+	if (!mgmt || memcmp(ap_compat_bssid, mgmt->bssid, ETH_ALEN))
+		return;
+
+	if (ieee80211_is_probe_resp(mgmt->frame_control))
+		ies = mgmt->u.probe_resp.variable;
+	else if (ieee80211_is_beacon(mgmt->frame_control))
+		ies = mgmt->u.beacon.variable;
+	else if (ieee80211_is_assoc_resp(mgmt->frame_control))
+		ies = mgmt->u.assoc_resp.variable;
+	else if (ieee80211_is_assoc_req(mgmt->frame_control))
+		ies = mgmt->u.assoc_req.variable;
+	else
+		return;
+
+	ies_len = skb->len - (ies - (u8 *)(skb->data));
+	ht_ie   = (u8 *)xradio_get_ie(ies, ies_len, WLAN_EID_HT_CAPABILITY);
+	if (ht_ie) {
+		u8 ht_len   = *(ht_ie + 1) + 2;
+		u8 move_len = (ies + ies_len) - (ht_ie + ht_len);
+		memmove(ht_ie, (ht_ie + ht_len), move_len);
+		skb_trim(skb, skb->len - ht_len);
+		ies_len = skb->len - (ies - (u8 *)(skb->data));
+		ht_ie = (u8 *)xradio_get_ie(ies, ies_len, WLAN_EID_HT_OPERATION);
+		if (ht_ie) {
+			ht_len   = *(ht_ie + 1) + 2;
+			move_len = (ies + ies_len) - (ht_ie + ht_len);
+			memmove(ht_ie, (ht_ie + ht_len), move_len);
+			skb_trim(skb, skb->len - ht_len);
+		}
+	}
+	txrx_printk(XRADIO_DBG_WARN, "%s: BSSID=%02x:%02x:%02x:%02x:%02x:%02x\n",
+	            __func__,
+	            mgmt->bssid[0], mgmt->bssid[1],
+	            mgmt->bssid[2], mgmt->bssid[3],
+	            mgmt->bssid[4], mgmt->bssid[5]);
+}
+#endif //AP_HT_COMPAT_FIX
\ No newline at end of file
diff --git a/drivers/net/wireless/xradio/ht.h b/drivers/net/wireless/xradio/ht.h
new file mode 100644
index 0000000..346b425
--- /dev/null
+++ b/drivers/net/wireless/xradio/ht.h
@@ -0,0 +1,44 @@
+/*
+ * HT-related code for XRadio drivers
+ *
+ * Copyright (c) 2013, XRadio
+ * Author: XRadio
+ *
+ * This program is free software; you can 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 XRADIO_HT_H_INCLUDED
+#define XRADIO_HT_H_INCLUDED
+
+#include <net/mac80211.h>
+
+struct xradio_ht_oper {
+	struct ieee80211_sta_ht_cap  ht_cap;
+	enum nl80211_channel_type    channel_type;
+	u16                          operation_mode;
+};
+
+static inline int xradio_is_ht(const struct xradio_ht_oper *ht_oper)
+{
+	return ht_oper->channel_type != NL80211_CHAN_NO_HT;
+}
+
+static inline int xradio_ht_greenfield(const struct xradio_ht_oper *ht_oper)
+{
+	return (xradio_is_ht(ht_oper) &&
+	       (ht_oper->ht_cap.cap      & IEEE80211_HT_CAP_GRN_FLD) &&
+	       !(ht_oper->operation_mode & IEEE80211_HT_OP_MODE_NON_GF_STA_PRSNT));
+}
+
+static inline int xradio_ht_ampdu_density(const struct xradio_ht_oper *ht_oper)
+{
+	if (!xradio_is_ht(ht_oper))
+		return 0;
+	return ht_oper->ht_cap.ampdu_density;
+}
+
+int xradio_apcompat_detect(struct xradio_vif *priv, u8 rx_rate);
+void xradio_remove_ht_ie(struct xradio_vif *priv, struct sk_buff *skb);
+
+#endif /* XRADIO_HT_H_INCLUDED */
diff --git a/drivers/net/wireless/xradio/hwio.c b/drivers/net/wireless/xradio/hwio.c
new file mode 100644
index 0000000..b813333
--- /dev/null
+++ b/drivers/net/wireless/xradio/hwio.c
@@ -0,0 +1,280 @@
+/*
+ * Hardware I/O implementation for XRadio drivers
+ *
+ * Copyright (c) 2013, XRadio
+ * Author: XRadio
+ *
+ * This program is free software; you can 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/types.h>
+
+#include "xradio.h"
+#include "hwio.h"
+#include "sdio.h"
+
+#define CHECK_ADDR_LEN  1
+
+ /* Sdio addr is 4*spi_addr */
+#define SPI_REG_ADDR_TO_SDIO(spi_reg_addr) ((spi_reg_addr) << 2)
+#define SDIO_ADDR17BIT(buf_id, mpf, rfu, reg_id_ofs) \
+				((((buf_id)    & 0x1F) << 7) \
+				| (((mpf)        & 1) << 6) \
+				| (((rfu)        & 1) << 5) \
+				| (((reg_id_ofs) & 0x1F) << 0))
+#define MAX_RETRY		3
+
+
+static int __xradio_read(struct xradio_common *hw_priv, u16 addr,
+                         void *buf, size_t buf_len, int buf_id)
+{
+	u16 addr_sdio;
+	u32 sdio_reg_addr_17bit ;
+
+#if (CHECK_ADDR_LEN)
+	/* Check if buffer is aligned to 4 byte boundary */
+	if (WARN_ON(((unsigned long)buf & 3) && (buf_len > 4))) {
+		dev_dbg(hw_priv->pdev, "buffer is not aligned.\n");
+		return -EINVAL;
+	}
+#endif
+
+	/* Convert to SDIO Register Address */
+	addr_sdio = SPI_REG_ADDR_TO_SDIO(addr);
+	sdio_reg_addr_17bit = SDIO_ADDR17BIT(buf_id, 0, 0, addr_sdio);
+	return sdio_data_read(hw_priv,
+	                                         sdio_reg_addr_17bit,
+	                                         buf, buf_len);
+}
+
+static int __xradio_write(struct xradio_common *hw_priv, u16 addr,
+                              const void *buf, size_t buf_len, int buf_id)
+{
+	u16 addr_sdio;
+	u32 sdio_reg_addr_17bit ;
+
+#if (CHECK_ADDR_LEN)
+	/* Check if buffer is aligned to 4 byte boundary */
+	if (WARN_ON(((unsigned long)buf & 3) && (buf_len > 4))) {
+		dev_err(hw_priv->pdev, "buffer is not aligned.\n");
+		return -EINVAL;
+	}
+#endif
+
+	/* Convert to SDIO Register Address */
+	addr_sdio = SPI_REG_ADDR_TO_SDIO(addr);
+	sdio_reg_addr_17bit = SDIO_ADDR17BIT(buf_id, 0, 0, addr_sdio);
+
+	return sdio_data_write(hw_priv,
+	                                          sdio_reg_addr_17bit,
+	                                          buf, buf_len);
+}
+
+static inline int __xradio_read_reg32(struct xradio_common *hw_priv,
+                                       u16 addr, u32 *val)
+{
+	return __xradio_read(hw_priv, addr, val, sizeof(val), 0);
+}
+
+static inline int __xradio_write_reg32(struct xradio_common *hw_priv,
+                                        u16 addr, u32 val)
+{
+	return __xradio_write(hw_priv, addr, &val, sizeof(val), 0);
+}
+
+int xradio_reg_read(struct xradio_common *hw_priv, u16 addr, 
+                    void *buf, size_t buf_len)
+{
+	int ret;
+	sdio_lock(hw_priv);
+	ret = __xradio_read(hw_priv, addr, buf, buf_len, 0);
+	sdio_unlock(hw_priv);
+	return ret;
+}
+
+int xradio_reg_write(struct xradio_common *hw_priv, u16 addr, 
+                     const void *buf, size_t buf_len)
+{
+	int ret;
+	sdio_lock(hw_priv);
+	ret = __xradio_write(hw_priv, addr, buf, buf_len, 0);
+	sdio_unlock(hw_priv);
+	return ret;
+}
+
+int xradio_data_read(struct xradio_common *hw_priv, void *buf, size_t buf_len)
+{
+	int ret, retry = 1;
+	sdio_lock(hw_priv);
+	{
+		int buf_id_rx = hw_priv->buf_id_rx;
+		while (retry <= MAX_RETRY) {
+			ret = __xradio_read(hw_priv, HIF_IN_OUT_QUEUE_REG_ID, buf,
+			                    buf_len, buf_id_rx + 1);
+			if (!ret) {
+				buf_id_rx = (buf_id_rx + 1) & 3;
+				hw_priv->buf_id_rx = buf_id_rx;
+				break;
+			} else {
+				//~dgp this retrying stuff is silly as it can crash the fw if there is nothing to read
+				dev_err(hw_priv->pdev, "data read error :%d - attempt %d of %d\n", ret, retry, MAX_RETRY);
+				retry++;
+				mdelay(1);
+			}
+		}
+	}
+	sdio_unlock(hw_priv);
+	return ret;
+}
+
+int xradio_data_write(struct xradio_common *hw_priv, const void *buf,
+                      size_t buf_len)
+{
+	int ret, retry = 1;
+	sdio_lock(hw_priv);
+	{
+		int buf_id_tx = hw_priv->buf_id_tx;
+		while (retry <= MAX_RETRY) {
+			ret = __xradio_write(hw_priv, HIF_IN_OUT_QUEUE_REG_ID, buf,
+			                     buf_len, buf_id_tx);
+			if (!ret) {
+				buf_id_tx = (buf_id_tx + 1) & 31;
+				hw_priv->buf_id_tx = buf_id_tx;
+				break;
+			} else {
+				dev_err(hw_priv->pdev, "data write error :%d - attempt %d - %d\n", ret, retry, MAX_RETRY);
+				retry++;
+				mdelay(1);
+			}
+		}
+	}
+	sdio_unlock(hw_priv);
+	return ret;
+}
+
+int xradio_indirect_read(struct xradio_common *hw_priv, u32 addr, void *buf,
+                         size_t buf_len, u32 prefetch, u16 port_addr)
+{
+	u32 val32 = 0;
+	int i, ret;
+
+	if ((buf_len / 2) >= 0x1000) {
+		dev_err(hw_priv->pdev, "Can't read more than 0xfff words.\n");
+		return -EINVAL;
+		goto out;
+	}
+
+	sdio_lock(hw_priv);
+	/* Write address */
+	ret = __xradio_write_reg32(hw_priv, HIF_SRAM_BASE_ADDR_REG_ID, addr);
+	if (ret < 0) {
+		dev_err(hw_priv->pdev, "Can't write address register.\n");
+		goto out;
+	}
+
+	/* Read CONFIG Register Value - We will read 32 bits */
+	ret = __xradio_read_reg32(hw_priv, HIF_CONFIG_REG_ID, &val32);
+	if (ret < 0) {
+		dev_err(hw_priv->pdev, "Can't read config register.\n");
+		goto out;
+	}
+
+	/* Set PREFETCH bit */
+	ret = __xradio_write_reg32(hw_priv, HIF_CONFIG_REG_ID, val32 | prefetch);
+	if (ret < 0) {
+		dev_err(hw_priv->pdev, "Can't write prefetch bit.\n");
+		goto out;
+	}
+
+	/* Check for PRE-FETCH bit to be cleared */
+	for (i = 0; i < 20; i++) {
+		ret = __xradio_read_reg32(hw_priv, HIF_CONFIG_REG_ID, &val32);
+		if (ret < 0) {
+			dev_err(hw_priv->pdev, "Can't check prefetch bit.\n");
+			goto out;
+		}
+		if (!(val32 & prefetch))
+			break;
+		mdelay(i);
+	}
+
+	if (val32 & prefetch) {
+		dev_err(hw_priv->pdev, "Prefetch bit is not cleared.\n");
+		goto out;
+	}
+
+	/* Read data port */
+	ret = __xradio_read(hw_priv, port_addr, buf, buf_len, 0);
+	if (ret < 0) {
+		dev_err(hw_priv->pdev, "Can't read data port.\n");
+		goto out;
+	}
+
+out:
+	sdio_unlock(hw_priv);
+	return ret;
+}
+
+int xradio_apb_write(struct xradio_common *hw_priv, u32 addr, const void *buf,
+                     size_t buf_len)
+{
+	int ret;
+
+	if ((buf_len / 2) >= 0x1000) {
+		dev_err(hw_priv->pdev, "Can't wrire more than 0xfff words.\n");
+		return -EINVAL;
+	}
+
+	sdio_lock(hw_priv);
+
+	/* Write address */
+	ret = __xradio_write_reg32(hw_priv, HIF_SRAM_BASE_ADDR_REG_ID, addr);
+	if (ret < 0) {
+		dev_err(hw_priv->pdev, "Can't write address register.\n");
+		goto out;
+	}
+
+	/* Write data port */
+	ret = __xradio_write(hw_priv, HIF_SRAM_DPORT_REG_ID, buf, buf_len, 0);
+	if (ret < 0) {
+		dev_err(hw_priv->pdev, "Can't write data port.\n");
+		goto out;
+	}
+
+out:
+	sdio_unlock(hw_priv);
+	return ret;
+}
+
+int xradio_ahb_write(struct xradio_common *hw_priv, u32 addr, const void *buf,
+                     size_t buf_len)
+{
+	int ret;
+
+	if ((buf_len / 2) >= 0x1000) {
+		dev_err(hw_priv->pdev, "Can't wrire more than 0xfff words.\n");
+		return -EINVAL;
+	}
+
+	sdio_lock(hw_priv);
+	
+	/* Write address */
+	ret = __xradio_write_reg32(hw_priv, HIF_SRAM_BASE_ADDR_REG_ID, addr);
+	if (ret < 0) {
+		dev_err(hw_priv->pdev, "Can't write address register.\n");
+		goto out;
+	}
+
+	/* Write data port */
+	ret = __xradio_write(hw_priv, HIF_AHB_DPORT_REG_ID, buf, buf_len, 0);
+	if (ret < 0) {
+		dev_err(hw_priv->pdev, "Can't write data port.\n");
+		goto out;
+	}
+
+out:
+	sdio_unlock(hw_priv);
+	return ret;
+}
diff --git a/drivers/net/wireless/xradio/hwio.h b/drivers/net/wireless/xradio/hwio.h
new file mode 100644
index 0000000..531c2b8
--- /dev/null
+++ b/drivers/net/wireless/xradio/hwio.h
@@ -0,0 +1,229 @@
+/*
+ * hardware interfaces for XRadio drivers
+ *
+ * Copyright (c) 2013, XRadio
+ * Author: XRadio
+ *
+ * This program is free software; you can 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 XRADIO_HWIO_H_INCLUDED
+#define XRADIO_HWIO_H_INCLUDED
+
+/* extern */ struct xradio_common;
+
+/* DPLL initial values */
+#define DPLL_INIT_VAL_XRADIO      (0x0EC4F121)
+
+/* Hardware Type Definitions */
+#define HIF_HW_TYPE_XRADIO        (1)
+
+
+/* boot loader start address in SRAM */
+#define DOWNLOAD_BOOT_LOADER_OFFSET   (0x00000000)
+/* 32K, 0x4000 to 0xDFFF */
+#define DOWNLOAD_FIFO_OFFSET          (0x00004000)
+/* 32K */
+#define DOWNLOAD_FIFO_SIZE            (0x00008000)
+/* 128 bytes, 0xFF80 to 0xFFFF */
+#define DOWNLOAD_CTRL_OFFSET          (0x0000FF80)
+#define DOWNLOAD_CTRL_DATA_DWORDS     (32-6)
+
+/* Download control area */
+struct download_cntl_t {
+	/* size of whole firmware file (including Cheksum), host init */
+	u32 ImageSize;
+	/* downloading flags */
+	u32 Flags;
+	/* No. of bytes put into the download, init & updated by host */
+	u32 Put;
+	/* last traced program counter, last ARM reg_pc */
+	u32 TracePc;
+	/* No. of bytes read from the download, host init, device updates */
+	u32 Get;
+	/* r0, boot losader status, host init to pending, device updates */
+	u32 Status;
+	/* Extra debug info, r1 to r14 if status=r0=DOWNLOAD_EXCEPTION */
+	u32 DebugData[DOWNLOAD_CTRL_DATA_DWORDS];
+};
+
+#define	DOWNLOAD_IMAGE_SIZE_REG		\
+	(DOWNLOAD_CTRL_OFFSET + offsetof(struct download_cntl_t, ImageSize))
+#define	DOWNLOAD_FLAGS_REG		\
+	(DOWNLOAD_CTRL_OFFSET + offsetof(struct download_cntl_t, Flags))
+#define DOWNLOAD_PUT_REG		\
+	(DOWNLOAD_CTRL_OFFSET + offsetof(struct download_cntl_t, Put))
+#define DOWNLOAD_TRACE_PC_REG		\
+	(DOWNLOAD_CTRL_OFFSET + offsetof(struct download_cntl_t, TracePc))
+#define	DOWNLOAD_GET_REG		\
+	(DOWNLOAD_CTRL_OFFSET + offsetof(struct download_cntl_t, Get))
+#define	DOWNLOAD_STATUS_REG		\
+	(DOWNLOAD_CTRL_OFFSET + offsetof(struct download_cntl_t, Status))
+#define DOWNLOAD_DEBUG_DATA_REG		\
+	(DOWNLOAD_CTRL_OFFSET + offsetof(struct download_cntl_t, DebugData))
+	
+#define DOWNLOAD_DEBUG_DATA_LEN   (108)
+#define DOWNLOAD_BLOCK_SIZE       (1024)
+
+/* For boot loader detection */
+#define DOWNLOAD_ARE_YOU_HERE     (0x87654321)
+#define DOWNLOAD_I_AM_HERE        (0x12345678)
+
+/* Download error code */
+#define DOWNLOAD_PENDING        (0xFFFFFFFF)
+#define DOWNLOAD_SUCCESS        (0)
+#define DOWNLOAD_EXCEPTION      (1)
+#define DOWNLOAD_ERR_MEM_1      (2)
+#define DOWNLOAD_ERR_MEM_2      (3)
+#define DOWNLOAD_ERR_SOFTWARE   (4)
+#define DOWNLOAD_ERR_FILE_SIZE  (5)
+#define DOWNLOAD_ERR_CHECKSUM   (6)
+#define DOWNLOAD_ERR_OVERFLOW   (7)
+#define DOWNLOAD_ERR_IMAGE      (8)
+#define DOWNLOAD_ERR_HOST       (9)
+#define DOWNLOAD_ERR_ABORT      (10)
+
+#define SYS_BASE_ADDR_SILICON      (0)
+#define AHB_MEMORY_ADDRESS         (SYS_BASE_ADDR_SILICON + 0x08000000)
+#define PAC_BASE_ADDRESS_SILICON   (SYS_BASE_ADDR_SILICON + 0x09000000)
+#define PAC_SHARED_MEMORY_SILICON  (PAC_BASE_ADDRESS_SILICON)
+#define APB_ADDR(addr)             (PAC_SHARED_MEMORY_SILICON + (addr))
+
+/* ***************************************************************
+*Device register definitions
+*************************************************************** */
+/* WBF - SPI Register Addresses */
+#define HIF_ADDR_ID_BASE             (0x0000)
+/* 16/32 bits */
+#define HIF_CONFIG_REG_ID            (0x0000)
+/* 16/32 bits */
+#define HIF_CONTROL_REG_ID           (0x0001)
+/* 16 bits, Q mode W/R */
+#define HIF_IN_OUT_QUEUE_REG_ID      (0x0002)
+/* 32 bits, AHB bus R/W */
+#define HIF_AHB_DPORT_REG_ID         (0x0003)
+/* 16/32 bits */
+#define HIF_SRAM_BASE_ADDR_REG_ID    (0x0004)
+/* 32 bits, APB bus R/W */
+#define HIF_SRAM_DPORT_REG_ID        (0x0005)
+/* 32 bits, t_settle/general */
+#define HIF_TSET_GEN_R_W_REG_ID      (0x0006)
+/* 16 bits, Q mode read, no length */
+#define HIF_FRAME_OUT_REG_ID         (0x0007)
+#define HIF_ADDR_ID_MAX              (HIF_FRAME_OUT_REG_ID)
+
+/* WBF - Control register bit set */
+/* next o/p length, bit 11 to 0 */
+#define HIF_CTRL_NEXT_LEN_MASK     (0x0FFF)
+#define HIF_CTRL_WUP_BIT           (BIT(12))
+#define HIF_CTRL_RDY_BIT           (BIT(13))
+#define HIF_CTRL_IRQ_ENABLE        (BIT(14))
+#define HIF_CTRL_RDY_ENABLE        (BIT(15))
+#define HIF_CTRL_IRQ_RDY_ENABLE    (BIT(14)|BIT(15))
+
+/* SPI Config register bit set */
+#define HIF_CONFIG_FRAME_BIT       (BIT(2))
+#define HIF_CONFIG_WORD_MODE_BITS  (BIT(3)|BIT(4))
+#define HIF_CONFIG_WORD_MODE_1     (BIT(3))
+#define HIF_CONFIG_WORD_MODE_2     (BIT(4))
+#define HIF_CONFIG_ERROR_0_BIT     (BIT(5))
+#define HIF_CONFIG_ERROR_1_BIT     (BIT(6))
+#define HIF_CONFIG_ERROR_2_BIT     (BIT(7))
+/* TBD: Sure??? */
+#define HIF_CONFIG_CSN_FRAME_BIT   (BIT(7))
+#define HIF_CONFIG_ERROR_3_BIT     (BIT(8))
+#define HIF_CONFIG_ERROR_4_BIT     (BIT(9))
+/* QueueM */
+#define HIF_CONFIG_ACCESS_MODE_BIT (BIT(10))
+/* AHB bus */
+#define HIF_CONFIG_AHB_PFETCH_BIT  (BIT(11))
+#define HIF_CONFIG_CPU_CLK_DIS_BIT (BIT(12))
+/* APB bus */
+#define HIF_CONFIG_PFETCH_BIT      (BIT(13))
+/* cpu reset */
+#define HIF_CONFIG_CPU_RESET_BIT   (BIT(14))
+#define HIF_CONFIG_CLEAR_INT_BIT   (BIT(15))
+
+/* For XRADIO the IRQ Enable and Ready Bits are in CONFIG register */
+#define HIF_CONF_IRQ_RDY_ENABLE	(BIT(16)|BIT(17))
+
+int xradio_data_read(struct xradio_common *hw_priv, void *buf, size_t buf_len);
+int xradio_data_write(struct xradio_common *hw_priv, const void *buf, size_t buf_len);
+int xradio_reg_read(struct xradio_common *hw_priv, u16 addr, void *buf, size_t buf_len);
+int xradio_reg_write(struct xradio_common *hw_priv, u16 addr, const void *buf, size_t buf_len);
+int xradio_indirect_read(struct xradio_common *hw_priv, u32 addr, void *buf, 
+                         size_t buf_len, u32 prefetch, u16 port_addr);
+int xradio_apb_write(struct xradio_common *hw_priv, u32 addr, const void *buf, size_t buf_len);
+int xradio_ahb_write(struct xradio_common *hw_priv, u32 addr, const void *buf, size_t buf_len);
+
+
+static inline int xradio_reg_read_16(struct xradio_common *hw_priv,
+                                     u16 addr, u16 *val)
+{
+	int ret    = 0;
+	u32 bigVal = 0;
+	ret = xradio_reg_read(hw_priv, addr, &bigVal, sizeof(bigVal));
+	*val = (u16)bigVal;
+	return ret;
+}
+
+static inline int xradio_reg_write_16(struct xradio_common *hw_priv,
+                                      u16 addr, u16 val)
+{
+	u32 bigVal = (u32)val;
+	return xradio_reg_write(hw_priv, addr, &bigVal, sizeof(bigVal));
+}
+
+static inline int xradio_reg_read_32(struct xradio_common *hw_priv,
+                                      u16 addr, u32 *val)
+{
+	return xradio_reg_read(hw_priv, addr, val, sizeof(val));
+}
+
+static inline int xradio_reg_write_32(struct xradio_common *hw_priv,
+                                      u16 addr, u32 val)
+{
+	return xradio_reg_write(hw_priv, addr, &val, sizeof(val));
+}
+
+static inline int xradio_apb_read(struct xradio_common *hw_priv, u32 addr,
+                                  void *buf, size_t buf_len)
+{
+	return xradio_indirect_read(hw_priv, addr, buf, buf_len, HIF_CONFIG_PFETCH_BIT, 
+	                            HIF_SRAM_DPORT_REG_ID);
+}
+
+static inline int xradio_ahb_read(struct xradio_common *hw_priv, u32 addr,
+                                  void *buf, size_t buf_len)
+{
+	return xradio_indirect_read(hw_priv, addr, buf, buf_len, HIF_CONFIG_AHB_PFETCH_BIT, 
+	                            HIF_AHB_DPORT_REG_ID);
+}
+
+static inline int xradio_apb_read_32(struct xradio_common *hw_priv,
+                                      u32 addr, u32 *val)
+{
+	return xradio_apb_read(hw_priv, addr, val, sizeof(val));
+}
+
+static inline int xradio_apb_write_32(struct xradio_common *hw_priv,
+                                      u32 addr, u32 val)
+{
+	return xradio_apb_write(hw_priv, addr, &val, sizeof(val));
+}
+
+static inline int xradio_ahb_read_32(struct xradio_common *hw_priv,
+                                      u32 addr, u32 *val)
+{
+	return xradio_ahb_read(hw_priv, addr, val, sizeof(val));
+}
+
+static inline int xradio_ahb_write_32(struct xradio_common *hw_priv,
+                                      u32 addr, u32 val)
+{
+	return xradio_ahb_write(hw_priv, addr, &val, sizeof(val));
+}
+
+#endif /* XRADIO_HWIO_H_INCLUDED */
diff --git a/drivers/net/wireless/xradio/keys.c b/drivers/net/wireless/xradio/keys.c
new file mode 100644
index 0000000..20050e1
--- /dev/null
+++ b/drivers/net/wireless/xradio/keys.c
@@ -0,0 +1,193 @@
+#include <net/mac80211.h>
+
+#include "xradio.h"
+#include "sta.h"
+#include "keys.h"
+
+int xradio_alloc_key(struct xradio_common *hw_priv)
+{
+	int idx;
+
+	idx = ffs(~hw_priv->key_map) - 1;
+	if (idx < 0 || idx > WSM_KEY_MAX_INDEX)
+		return -1;
+
+	hw_priv->key_map |= BIT(idx);
+	hw_priv->keys[idx].entryIndex = idx;
+	return idx;
+}
+
+void xradio_free_key(struct xradio_common *hw_priv, int idx)
+{
+	BUG_ON(!(hw_priv->key_map & BIT(idx)));
+	memset(&hw_priv->keys[idx], 0, sizeof(hw_priv->keys[idx]));
+	hw_priv->key_map &= ~BIT(idx);
+}
+
+void xradio_free_keys(struct xradio_common *hw_priv)
+{
+	memset(&hw_priv->keys, 0, sizeof(hw_priv->keys));
+	hw_priv->key_map = 0;
+}
+
+int xradio_upload_keys(struct xradio_vif *priv)
+{
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	int idx, ret = 0;
+
+
+	for (idx = 0; idx <= WSM_KEY_MAX_IDX; ++idx)
+		if (hw_priv->key_map & BIT(idx)) {
+			ret = wsm_add_key(hw_priv, &hw_priv->keys[idx], priv->if_id);
+			if (ret < 0)
+				break;
+		}
+	return ret;
+}
+
+int xradio_set_key(struct ieee80211_hw *dev, enum set_key_cmd cmd,
+                   struct ieee80211_vif *vif, struct ieee80211_sta *sta,
+                   struct ieee80211_key_conf *key)
+{
+#ifdef XRADIO_DISABLE_HW_CRYPTO
+	wiphy_info(dev->wiphy, "hw crypto is disabled, ignoring key request\n");
+	return -EOPNOTSUPP;
+#else
+	int ret = -EOPNOTSUPP;
+	struct xradio_common *hw_priv = dev->priv;
+	struct xradio_vif *priv = xrwl_get_vif_from_ieee80211(vif);
+
+	wiphy_dbg(dev->wiphy, "vif %d: set_key cmd %d\n", priv->if_id, (int) cmd);
+	
+	mutex_lock(&hw_priv->conf_mutex);
+
+	if (cmd == SET_KEY) {
+		u8 *peer_addr = NULL;
+		int pairwise = (key->flags & IEEE80211_KEY_FLAG_PAIRWISE) ? 1 : 0;
+		int idx = xradio_alloc_key(hw_priv);
+		struct wsm_add_key *wsm_key = &hw_priv->keys[idx];
+
+		if (idx < 0) {
+			wiphy_err(dev->wiphy, "xradio_alloc_key failed!\n");
+			ret = -EINVAL;
+			goto finally;
+		}
+
+		BUG_ON(pairwise && !sta);
+		if (sta)
+			peer_addr = sta->addr;
+
+		key->flags |= IEEE80211_KEY_FLAG_PUT_IV_SPACE;
+
+		priv->cipherType = key->cipher;
+		switch (key->cipher) {
+		case WLAN_CIPHER_SUITE_WEP40:
+		case WLAN_CIPHER_SUITE_WEP104:
+			if (key->keylen > 16) {
+				xradio_free_key(hw_priv, idx);
+				wiphy_err(dev->wiphy, "keylen too long=%d!\n", key->keylen);
+				ret = -EINVAL;
+				goto finally;
+			}
+
+			if (pairwise) {
+				wsm_key->type = WSM_KEY_TYPE_WEP_PAIRWISE;
+				memcpy(wsm_key->wepPairwiseKey.peerAddress, peer_addr, ETH_ALEN);
+				memcpy(wsm_key->wepPairwiseKey.keyData, &key->key[0], key->keylen);
+				wsm_key->wepPairwiseKey.keyLength = key->keylen;
+			} else {
+				wsm_key->type = WSM_KEY_TYPE_WEP_DEFAULT;
+				memcpy(wsm_key->wepGroupKey.keyData, &key->key[0], key->keylen);
+				wsm_key->wepGroupKey.keyLength = key->keylen;
+				wsm_key->wepGroupKey.keyId     = key->keyidx;
+			}
+			break;
+		case WLAN_CIPHER_SUITE_TKIP:
+			if (pairwise) {
+				wsm_key->type = WSM_KEY_TYPE_TKIP_PAIRWISE;
+				memcpy(wsm_key->tkipPairwiseKey.peerAddress, peer_addr, ETH_ALEN);
+				memcpy(wsm_key->tkipPairwiseKey.tkipKeyData, &key->key[0], 16);
+				memcpy(wsm_key->tkipPairwiseKey.txMicKey, &key->key[16], 8);
+				memcpy(wsm_key->tkipPairwiseKey.rxMicKey, &key->key[24], 8);
+			} else {
+				size_t mic_offset = (priv->mode == NL80211_IFTYPE_AP) ? 16 : 24;
+				wsm_key->type = WSM_KEY_TYPE_TKIP_GROUP;
+				memcpy(wsm_key->tkipGroupKey.tkipKeyData,&key->key[0],  16);
+				memcpy(wsm_key->tkipGroupKey.rxMicKey, &key->key[mic_offset], 8);
+
+				/* TODO: Where can I find TKIP SEQ? */
+				memset(wsm_key->tkipGroupKey.rxSeqCounter, 0, 8);
+				wsm_key->tkipGroupKey.keyId = key->keyidx;
+			}
+			break;
+		case WLAN_CIPHER_SUITE_CCMP:
+			if (pairwise) {
+				wsm_key->type = WSM_KEY_TYPE_AES_PAIRWISE;
+				memcpy(wsm_key->aesPairwiseKey.peerAddress, peer_addr, ETH_ALEN);
+				memcpy(wsm_key->aesPairwiseKey.aesKeyData, &key->key[0], 16);
+				wiphy_debug(dev->wiphy, "CCMP_PAIRWISE keylen=%d!\n",
+						key->keylen);
+			} else {
+				wsm_key->type = WSM_KEY_TYPE_AES_GROUP;
+				memcpy(wsm_key->aesGroupKey.aesKeyData, &key->key[0], 16);
+				/* TODO: Where can I find AES SEQ? */
+				memset(wsm_key->aesGroupKey.rxSeqCounter, 0, 8);
+				wsm_key->aesGroupKey.keyId = key->keyidx;
+			}
+			break;
+#ifdef CONFIG_XRADIO_WAPI_SUPPORT
+		case WLAN_CIPHER_SUITE_SMS4:
+			if (pairwise) {
+				wsm_key->type = WSM_KEY_TYPE_WAPI_PAIRWISE;
+				memcpy(wsm_key->wapiPairwiseKey.peerAddress, peer_addr, ETH_ALEN);
+				memcpy(wsm_key->wapiPairwiseKey.wapiKeyData, &key->key[0],  16);
+				memcpy(wsm_key->wapiPairwiseKey.micKeyData, &key->key[16], 16);
+				wsm_key->wapiPairwiseKey.keyId = key->keyidx;
+				sta_printk(XRADIO_DBG_NIY,"%s: WAPI_PAIRWISE keylen=%d!\n",
+			               __func__, key->keylen);
+			} else {
+				wsm_key->type = WSM_KEY_TYPE_WAPI_GROUP;
+				memcpy(wsm_key->wapiGroupKey.wapiKeyData, &key->key[0],  16);
+				memcpy(wsm_key->wapiGroupKey.micKeyData,  &key->key[16], 16);
+				wsm_key->wapiGroupKey.keyId = key->keyidx;
+				sta_printk(XRADIO_DBG_NIY,"%s: WAPI_GROUP keylen=%d!\n",
+			               __func__, key->keylen);
+			}
+			break;
+#endif /* CONFIG_XRADIO_WAPI_SUPPORT */
+		default:
+			wiphy_err(dev->wiphy, "key->cipher unknown(%d)!\n", key->cipher);
+			xradio_free_key(hw_priv, idx);
+			ret = -EOPNOTSUPP;
+			goto finally;
+		}
+		ret = WARN_ON(wsm_add_key(hw_priv, wsm_key, priv->if_id));
+		if (!ret)
+			key->hw_key_idx = idx;
+		else
+			xradio_free_key(hw_priv, idx);
+
+		if (!ret && (pairwise || wsm_key->type == WSM_KEY_TYPE_WEP_DEFAULT) && 
+		    (priv->filter4.enable & 0x2))
+			xradio_set_arpreply(dev, vif);
+	} else if (cmd == DISABLE_KEY) {
+		struct wsm_remove_key wsm_key = {
+			.entryIndex = key->hw_key_idx,
+		};
+
+		if (wsm_key.entryIndex > WSM_KEY_MAX_IDX) {
+			ret = -EINVAL;
+			goto finally;
+		}
+
+		xradio_free_key(hw_priv, wsm_key.entryIndex);
+		ret = wsm_remove_key(hw_priv, &wsm_key, priv->if_id);
+	} else {
+		wiphy_err(dev->wiphy, "Unsupported command\n");
+	}
+
+finally:
+	mutex_unlock(&hw_priv->conf_mutex);
+	return ret;
+#endif // XRADIO_DISABLE_HW_CRYPTO
+}
diff --git a/drivers/net/wireless/xradio/keys.h b/drivers/net/wireless/xradio/keys.h
new file mode 100644
index 0000000..23c5880
--- /dev/null
+++ b/drivers/net/wireless/xradio/keys.h
@@ -0,0 +1,12 @@
+#ifndef __KEYS_H_
+#define __KEYS_H_INCLUDED
+
+int xradio_alloc_key(struct xradio_common *hw_priv);
+void xradio_free_key(struct xradio_common *hw_priv, int idx);
+void xradio_free_keys(struct xradio_common *hw_priv);
+int xradio_upload_keys(struct xradio_vif *priv);
+int xradio_set_key(struct ieee80211_hw *dev, enum set_key_cmd cmd,
+                   struct ieee80211_vif *vif, struct ieee80211_sta *sta,
+                   struct ieee80211_key_conf *key);
+
+#endif
\ No newline at end of file
diff --git a/drivers/net/wireless/xradio/main.c b/drivers/net/wireless/xradio/main.c
new file mode 100644
index 0000000..346710c
--- /dev/null
+++ b/drivers/net/wireless/xradio/main.c
@@ -0,0 +1,609 @@
+/*
+ * Main code of XRadio drivers
+ *
+ * Copyright (c) 2013, XRadio
+ * Author: XRadio
+ *
+ * This program is free software; you can 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/firmware.h>
+#include <net/cfg80211.h>
+#include <linux/of_net.h>
+#include <linux/mmc/sdio_func.h>
+
+#include "xradio.h"
+#include "fwio.h"
+#include "hwio.h"
+#include "bh.h"
+#include "sta.h"
+#include "ap.h"
+#include "keys.h"
+#include "scan.h"
+#include "pm.h"
+#include "sdio.h"
+
+/* TODO: use rates and channels from the device */
+#define RATETAB_ENT(_rate, _rateid, _flags)		\
+	{						\
+		.bitrate  = (_rate),    \
+		.hw_value = (_rateid),  \
+		.flags    = (_flags),   \
+	}
+
+static struct ieee80211_rate xradio_rates[] = {
+	RATETAB_ENT(10,  0,   0),
+	RATETAB_ENT(20,  1,   0),
+	RATETAB_ENT(55,  2,   0),
+	RATETAB_ENT(110, 3,   0),
+	RATETAB_ENT(60,  6,  0),
+	RATETAB_ENT(90,  7,  0),
+	RATETAB_ENT(120, 8,  0),
+	RATETAB_ENT(180, 9,  0),
+	RATETAB_ENT(240, 10, 0),
+	RATETAB_ENT(360, 11, 0),
+	RATETAB_ENT(480, 12, 0),
+	RATETAB_ENT(540, 13, 0),
+};
+
+static struct ieee80211_rate xradio_mcs_rates[] = {
+	RATETAB_ENT(65,  14, IEEE80211_TX_RC_MCS),
+	RATETAB_ENT(130, 15, IEEE80211_TX_RC_MCS),
+	RATETAB_ENT(195, 16, IEEE80211_TX_RC_MCS),
+	RATETAB_ENT(260, 17, IEEE80211_TX_RC_MCS),
+	RATETAB_ENT(390, 18, IEEE80211_TX_RC_MCS),
+	RATETAB_ENT(520, 19, IEEE80211_TX_RC_MCS),
+	RATETAB_ENT(585, 20, IEEE80211_TX_RC_MCS),
+	RATETAB_ENT(650, 21, IEEE80211_TX_RC_MCS),
+};
+
+#define xradio_g_rates      (xradio_rates + 0)
+#define xradio_a_rates      (xradio_rates + 4)
+#define xradio_n_rates      (xradio_mcs_rates)
+
+#define xradio_g_rates_size (ARRAY_SIZE(xradio_rates))
+#define xradio_a_rates_size (ARRAY_SIZE(xradio_rates) - 4)
+#define xradio_n_rates_size (ARRAY_SIZE(xradio_mcs_rates))
+
+#define CHAN2G(_channel, _freq, _flags) {   \
+	.band             = NL80211_BAND_2GHZ,  \
+	.center_freq      = (_freq),              \
+	.hw_value         = (_channel),           \
+	.flags            = (_flags),             \
+	.max_antenna_gain = 0,                    \
+	.max_power        = 30,                   \
+}
+
+#define CHAN5G(_channel, _flags) {   \
+	.band             = NL80211_BAND_5GHZ,     \
+	.center_freq      = 5000 + (5 * (_channel)), \
+	.hw_value         = (_channel),              \
+	.flags            = (_flags),                \
+	.max_antenna_gain = 0,                       \
+	.max_power        = 30,                      \
+}
+
+static struct ieee80211_channel xradio_2ghz_chantable[] = {
+	CHAN2G(1, 2412, 0),
+	CHAN2G(2, 2417, 0),
+	CHAN2G(3, 2422, 0),
+	CHAN2G(4, 2427, 0),
+	CHAN2G(5, 2432, 0),
+	CHAN2G(6, 2437, 0),
+	CHAN2G(7, 2442, 0),
+	CHAN2G(8, 2447, 0),
+	CHAN2G(9, 2452, 0),
+	CHAN2G(10, 2457, 0),
+	CHAN2G(11, 2462, 0),
+	CHAN2G(12, 2467, 0),
+	CHAN2G(13, 2472, 0),
+	CHAN2G(14, 2484, 0),
+};
+
+static struct ieee80211_supported_band xradio_band_2ghz = {
+	.channels = xradio_2ghz_chantable,
+	.n_channels = ARRAY_SIZE(xradio_2ghz_chantable),
+	.bitrates = xradio_g_rates,
+	.n_bitrates = xradio_g_rates_size,
+	.ht_cap = {
+		.cap = IEEE80211_HT_CAP_GRN_FLD |
+		       (1 << IEEE80211_HT_CAP_RX_STBC_SHIFT),
+		.ht_supported  = 1,
+		.ampdu_factor  = IEEE80211_HT_MAX_AMPDU_32K,
+		.ampdu_density = IEEE80211_HT_MPDU_DENSITY_NONE,
+		.mcs = {
+			.rx_mask[0] = 0xFF,
+			.rx_highest = __cpu_to_le16(0x41),
+			.tx_params  = IEEE80211_HT_MCS_TX_DEFINED,
+		},
+	},
+};
+
+static const unsigned long xradio_ttl[] = {
+	1 * HZ,	/* VO */
+	2 * HZ,	/* VI */
+	5 * HZ, /* BE */
+	10 * HZ	/* BK */
+};
+
+static const struct ieee80211_ops xradio_ops = {
+	.start             = xradio_start,
+	.stop              = xradio_stop,
+	.add_interface     = xradio_add_interface,
+	.remove_interface  = xradio_remove_interface,
+	.change_interface  = xradio_change_interface,
+	.tx                = xradio_tx,
+	.hw_scan           = xradio_hw_scan,
+#ifdef ROAM_OFFLOAD
+	.sched_scan_start  = xradio_hw_sched_scan_start,
+	.sched_scan_stop   = xradio_hw_sched_scan_stop,
+#endif /*ROAM_OFFLOAD*/
+	.set_tim           = xradio_set_tim,
+	.sta_notify        = xradio_sta_notify,
+	.sta_add           = xradio_sta_add,
+	.sta_remove        = xradio_sta_remove,
+	.set_key           = xradio_set_key,
+	.set_rts_threshold = xradio_set_rts_threshold,
+	.config            = xradio_config,
+	.bss_info_changed  = xradio_bss_info_changed,
+	.prepare_multicast = xradio_prepare_multicast,
+	.configure_filter  = xradio_configure_filter,
+	.conf_tx           = xradio_conf_tx,
+	.get_stats         = xradio_get_stats,
+	.ampdu_action      = xradio_ampdu_action,
+	.flush             = xradio_flush,
+#ifdef CONFIG_PM
+	.suspend           = xradio_wow_suspend,
+	.resume            = xradio_wow_resume,
+#endif /* CONFIG_PM */
+	/* Intentionally not offloaded:					*/
+	/*.channel_switch	 = xradio_channel_switch,		*/
+	.remain_on_channel = xradio_remain_on_channel,
+	.cancel_remain_on_channel = xradio_cancel_remain_on_channel,
+};
+
+
+/*************************************** functions ***************************************/
+
+static void xradio_set_ifce_comb(struct xradio_common *hw_priv,
+				 struct ieee80211_hw *hw)
+{
+	hw_priv->if_limits1[0].max = 1;
+
+	hw_priv->if_limits1[0].types = BIT(NL80211_IFTYPE_STATION);
+	hw_priv->if_limits1[1].max = 1;
+	hw_priv->if_limits1[1].types = BIT(NL80211_IFTYPE_AP);
+
+	hw_priv->if_limits2[0].max = 2;
+	hw_priv->if_limits2[0].types = BIT(NL80211_IFTYPE_STATION);
+
+	hw_priv->if_limits3[0].max = 1;
+
+	hw_priv->if_limits3[0].types = BIT(NL80211_IFTYPE_STATION);
+	hw_priv->if_limits3[1].max = 1;
+	hw_priv->if_limits3[1].types = BIT(NL80211_IFTYPE_P2P_CLIENT) |
+				      BIT(NL80211_IFTYPE_P2P_GO);
+
+	/* TODO:COMBO: mac80211 doesn't yet support more than 1
+	 * different channel */
+	hw_priv->if_combs[0].num_different_channels = 1;
+	hw_priv->if_combs[0].max_interfaces = 2;
+	hw_priv->if_combs[0].limits = hw_priv->if_limits1;
+	hw_priv->if_combs[0].n_limits = 2;
+
+	hw_priv->if_combs[1].num_different_channels = 1;
+
+	hw_priv->if_combs[1].max_interfaces = 2;
+	hw_priv->if_combs[1].limits = hw_priv->if_limits2;
+	hw_priv->if_combs[1].n_limits = 1;
+
+	hw_priv->if_combs[2].num_different_channels = 1;
+	hw_priv->if_combs[2].max_interfaces = 2;
+	hw_priv->if_combs[2].limits = hw_priv->if_limits3;
+	hw_priv->if_combs[2].n_limits = 2;
+
+	hw->wiphy->iface_combinations = &hw_priv->if_combs[0];
+	hw->wiphy->n_iface_combinations = 3;
+}
+
+struct ieee80211_hw *xradio_init_common(size_t hw_priv_data_len)
+{
+	int i;
+	struct ieee80211_hw *hw;
+	struct xradio_common *hw_priv;
+	struct ieee80211_supported_band *sband;
+	int band;
+
+	/* Alloc ieee_802.11 hw and xradio_common struct. */
+	hw = ieee80211_alloc_hw(hw_priv_data_len, &xradio_ops);
+	if (!hw)
+		return NULL;
+	hw_priv = hw->priv;
+	memset(hw_priv, 0, sizeof(*hw_priv));
+
+	/* Initialize members of hw_priv. */
+	hw_priv->hw = hw;
+	hw_priv->if_id_slot = 0;
+	hw_priv->roc_if_id = -1;
+	atomic_set(&hw_priv->num_vifs, 0);
+	/* initial rates and channels TODO: fetch from FW */
+	hw_priv->rates = xradio_rates;    
+	hw_priv->mcs_rates = xradio_n_rates;
+#ifdef ROAM_OFFLOAD
+	hw_priv->auto_scanning = 0;
+	hw_priv->frame_rcvd = 0;
+	hw_priv->num_scanchannels = 0;
+	hw_priv->num_2g_channels = 0;
+	hw_priv->num_5g_channels = 0;
+#endif /*ROAM_OFFLOAD*/
+#ifdef AP_AGGREGATE_FW_FIX
+	/* Enable block ACK for 4 TID (BE,VI,VI,VO). */
+	hw_priv->ba_tid_mask = 0xB1;  /*due to HW limitations*/
+#else
+	/* Enable block ACK for every TID but voice. */
+	hw_priv->ba_tid_mask = 0x3F;
+#endif
+	hw_priv->noise = -94;
+	/* hw_priv->beacon_req_id = cpu_to_le32(0); */
+
+	/* Initialize members of ieee80211_hw, it works in UMAC. */
+	hw->sta_data_size = sizeof(struct xradio_sta_priv);
+	hw->vif_data_size = sizeof(struct xradio_vif);
+
+	ieee80211_hw_set(hw, SIGNAL_DBM);
+	ieee80211_hw_set(hw, SUPPORTS_PS);
+	ieee80211_hw_set(hw, SUPPORTS_DYNAMIC_PS);
+	ieee80211_hw_set(hw, REPORTS_TX_ACK_STATUS);
+	ieee80211_hw_set(hw, CONNECTION_MONITOR);
+
+/*	hw->flags = IEEE80211_HW_SIGNAL_DBM            |
+	            IEEE80211_HW_SUPPORTS_PS           |
+	            IEEE80211_HW_SUPPORTS_DYNAMIC_PS   |
+	            IEEE80211_HW_REPORTS_TX_ACK_STATUS |
+	            IEEE80211_HW_CONNECTION_MONITOR;*/
+	            //IEEE80211_HW_SUPPORTS_CQM_RSSI     |
+	            /* Aggregation is fully controlled by firmware.
+	             * Do not need any support from the mac80211 stack */
+	            /* IEEE80211_HW_AMPDU_AGGREGATION  | */
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+	            //IEEE80211_HW_SUPPORTS_P2P_PS          |
+	            //IEEE80211_HW_SUPPORTS_CQM_BEACON_MISS |
+	          //  IEEE80211_HW_SUPPORTS_CQM_TX_FAIL     |
+#endif /* CONFIG_XRADIO_USE_EXTENSIONS */
+	            //IEEE80211_HW_BEACON_FILTER;
+
+	hw->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION)    |
+	                             BIT(NL80211_IFTYPE_ADHOC)      |
+	                             BIT(NL80211_IFTYPE_AP)         |
+	                             BIT(NL80211_IFTYPE_MESH_POINT) |
+	                             BIT(NL80211_IFTYPE_P2P_CLIENT) |
+	                             BIT(NL80211_IFTYPE_P2P_GO);
+
+	/* Support only for limited wowlan functionalities */
+	/* TODO by Icenowy: RESTORE THIS */
+/*	hw->wiphy->wowlan.flags = WIPHY_WOWLAN_ANY | WIPHY_WOWLAN_DISCONNECT;
+	hw->wiphy->wowlan.n_patterns = 0;*/
+
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+	hw->wiphy->flags |= WIPHY_FLAG_AP_UAPSD;
+#endif /* CONFIG_XRADIO_USE_EXTENSIONS */
+	/* fix the problem that driver can not set pro-resp template frame to fw */
+	hw->wiphy->flags |= WIPHY_FLAG_AP_PROBE_RESP_OFFLOAD;
+
+#if defined(CONFIG_XRADIO_DISABLE_BEACON_HINTS)
+	hw->wiphy->flags |= WIPHY_FLAG_DISABLE_BEACON_HINTS;
+#endif
+	hw->wiphy->n_addresses = XRWL_MAX_VIFS;
+	hw->wiphy->addresses   = hw_priv->addresses;
+	hw->wiphy->max_remain_on_channel_duration = 500;
+	hw->extra_tx_headroom = WSM_TX_EXTRA_HEADROOM +
+	                        8  /* TKIP IV */      +
+	                        12 /* TKIP ICV and MIC */;
+	hw->wiphy->bands[NL80211_BAND_2GHZ] = &xradio_band_2ghz;
+	hw->queues         = AC_QUEUE_NUM;
+	hw->max_rates      = MAX_RATES_STAGE;
+	hw->max_rate_tries = MAX_RATES_RETRY;
+	/* Channel params have to be cleared before registering wiphy again */
+	for (band = 0; band < NUM_NL80211_BANDS; band++) {
+		sband = hw->wiphy->bands[band];
+		if (!sband)
+			continue;
+		for (i = 0; i < sband->n_channels; i++) {
+			sband->channels[i].flags = 0;
+			sband->channels[i].max_antenna_gain = 0;
+			sband->channels[i].max_power = 30;
+		}
+	}
+	/* hw_priv->channel init value is the local->oper_channel init value;when transplanting,take care */
+	for (band = 0; band < NUM_NL80211_BANDS; band++) {
+		sband = hw->wiphy->bands[band];
+		if (!sband)
+			continue;
+		if(!hw_priv->channel){
+			hw_priv->channel = &sband->channels[2];
+		}	
+	}
+	hw->wiphy->max_scan_ssids = WSM_SCAN_MAX_NUM_OF_SSIDS;
+	hw->wiphy->max_scan_ie_len = IEEE80211_MAX_DATA_LEN;
+	SET_IEEE80211_PERM_ADDR(hw, hw_priv->addresses[0].addr);
+
+	/* Initialize locks. */
+	spin_lock_init(&hw_priv->vif_list_lock);
+	mutex_init(&hw_priv->wsm_cmd_mux);
+	mutex_init(&hw_priv->conf_mutex);
+	mutex_init(&hw_priv->wsm_oper_lock);
+	atomic_set(&hw_priv->tx_lock, 0);
+	sema_init(&hw_priv->tx_lock_sem, 1);
+
+	hw_priv->workqueue = create_singlethread_workqueue(XRADIO_WORKQUEUE);
+	sema_init(&hw_priv->scan.lock, 1);
+	sema_init(&hw_priv->scan.status_lock,1);
+	INIT_WORK(&hw_priv->scan.work, xradio_scan_work);
+#ifdef ROAM_OFFLOAD
+	INIT_WORK(&hw_priv->scan.swork, xradio_sched_scan_work);
+#endif /*ROAM_OFFLOAD*/
+	INIT_DELAYED_WORK(&hw_priv->scan.probe_work, xradio_probe_work);
+	INIT_DELAYED_WORK(&hw_priv->scan.timeout, xradio_scan_timeout);
+	INIT_DELAYED_WORK(&hw_priv->rem_chan_timeout, xradio_rem_chan_timeout);
+	INIT_WORK(&hw_priv->tx_policy_upload_work, tx_policy_upload_work);
+	atomic_set(&hw_priv->upload_count, 0);
+	memset(&hw_priv->connet_time, 0, sizeof(hw_priv->connet_time));
+
+	spin_lock_init(&hw_priv->event_queue_lock);
+	INIT_LIST_HEAD(&hw_priv->event_queue);
+	INIT_WORK(&hw_priv->event_handler, xradio_event_handler);
+	INIT_WORK(&hw_priv->ba_work, xradio_ba_work);
+	spin_lock_init(&hw_priv->ba_lock);
+	timer_setup(&hw_priv->ba_timer, xradio_ba_timer, 0);
+
+	if (unlikely(xradio_queue_stats_init(&hw_priv->tx_queue_stats,
+			WLAN_LINK_ID_MAX,xradio_skb_dtor, hw_priv))) {
+		ieee80211_free_hw(hw);
+		return NULL;
+	}
+	for (i = 0; i < AC_QUEUE_NUM; ++i) {
+		if (unlikely(xradio_queue_init(&hw_priv->tx_queue[i],
+				&hw_priv->tx_queue_stats, i, XRWL_MAX_QUEUE_SZ, xradio_ttl[i]))) {
+			for (; i > 0; i--)
+				xradio_queue_deinit(&hw_priv->tx_queue[i - 1]);
+			xradio_queue_stats_deinit(&hw_priv->tx_queue_stats);
+			ieee80211_free_hw(hw);
+			return NULL;
+		}
+	}
+
+	init_waitqueue_head(&hw_priv->channel_switch_done);
+	init_waitqueue_head(&hw_priv->wsm_cmd_wq);
+	init_waitqueue_head(&hw_priv->wsm_startup_done);
+	init_waitqueue_head(&hw_priv->offchannel_wq);
+	hw_priv->wsm_caps.firmwareReady = 0;
+	hw_priv->driver_ready = 0;
+	hw_priv->offchannel_done = 0;
+	wsm_buf_init(&hw_priv->wsm_cmd_buf);
+	spin_lock_init(&hw_priv->wsm_cmd.lock);
+	tx_policy_init(hw_priv);
+	xradio_init_resv_skb(hw_priv);
+	/* add for setting short_frame_max_tx_count(mean wdev->retry_short) to drv,init the max_rate_tries */
+	spin_lock_bh(&hw_priv->tx_policy_cache.lock);
+	hw_priv->long_frame_max_tx_count = hw->conf.long_frame_max_tx_count;
+	hw_priv->short_frame_max_tx_count =
+			(hw->conf.short_frame_max_tx_count< 0x0F) ?
+			hw->conf.short_frame_max_tx_count : 0x0F;
+	hw_priv->hw->max_rate_tries = hw->conf.short_frame_max_tx_count;
+	spin_unlock_bh(&hw_priv->tx_policy_cache.lock);
+
+	for (i = 0; i < XRWL_MAX_VIFS; i++)
+		hw_priv->hw_bufs_used_vif[i] = 0;
+
+#ifdef MCAST_FWDING
+	for (i = 0; i < WSM_MAX_BUF; i++)
+		wsm_init_release_buffer_request(hw_priv, i);
+	hw_priv->buf_released = 0;
+#endif
+	hw_priv->vif0_throttle = XRWL_HOST_VIF0_11BG_THROTTLE;
+	hw_priv->vif1_throttle = XRWL_HOST_VIF1_11BG_THROTTLE;
+
+	hw_priv->query_packetID = 0;
+	atomic_set(&hw_priv->query_cnt, 0);
+	INIT_WORK(&hw_priv->query_work, wsm_query_work);
+
+#ifdef CONFIG_XRADIO_SUSPEND_POWER_OFF
+	atomic_set(&hw_priv->suspend_state, XRADIO_RESUME);
+#endif
+
+	xradio_set_ifce_comb(hw_priv, hw_priv->hw);
+
+	return hw;
+}
+
+void xradio_free_common(struct ieee80211_hw *dev)
+{
+	int i;
+	struct xradio_common *hw_priv = dev->priv;
+
+	cancel_work_sync(&hw_priv->query_work);
+	del_timer_sync(&hw_priv->ba_timer);
+	mutex_destroy(&hw_priv->wsm_oper_lock);
+	mutex_destroy(&hw_priv->conf_mutex);
+	mutex_destroy(&hw_priv->wsm_cmd_mux);
+	wsm_buf_deinit(&hw_priv->wsm_cmd_buf);
+	flush_workqueue(hw_priv->workqueue);
+	destroy_workqueue(hw_priv->workqueue);
+	hw_priv->workqueue = NULL;
+
+	xradio_deinit_resv_skb(hw_priv);
+	if (hw_priv->skb_cache) {
+		dev_kfree_skb(hw_priv->skb_cache);
+		hw_priv->skb_cache = NULL;
+	}
+
+	for (i = 0; i < 4; ++i)
+		xradio_queue_deinit(&hw_priv->tx_queue[i]);
+	xradio_queue_stats_deinit(&hw_priv->tx_queue_stats);
+
+	for (i = 0; i < XRWL_MAX_VIFS; i++) {
+		kfree(hw_priv->vif_list[i]);
+		hw_priv->vif_list[i] = NULL;
+	}
+
+//fixed memory leakage by yangfh
+#ifdef MCAST_FWDING
+	wsm_deinit_release_buffer(hw_priv);
+#endif
+	/* unsigned int i; */
+	ieee80211_free_hw(dev);
+}
+
+int xradio_register_common(struct ieee80211_hw *dev)
+{
+	int err = 0;
+	struct xradio_common *hw_priv = dev->priv;
+
+	SET_IEEE80211_DEV(dev, hw_priv->pdev);
+	err = ieee80211_register_hw(dev);
+	if (err) {
+		dev_dbg(hw_priv->pdev, "Cannot register device (%d).\n", err);
+		return err;
+	}
+	dev_dbg(hw_priv->pdev, "is registered as '%s'\n",
+	           wiphy_name(dev->wiphy));
+
+	hw_priv->driver_ready = 1;
+	wake_up(&hw_priv->wsm_startup_done);
+	return 0;
+}
+
+void xradio_unregister_common(struct ieee80211_hw *dev)
+{
+	struct xradio_common *hw_priv = dev->priv;
+
+	if (wiphy_dev(dev->wiphy)) {
+	ieee80211_unregister_hw(dev);
+		SET_IEEE80211_DEV(dev, NULL);
+	}
+	hw_priv->driver_ready = 0;
+}
+
+int xradio_core_init(struct sdio_func* func)
+{
+	int err = -ENOMEM;
+	u16 ctrl_reg;
+	int if_id;
+	struct ieee80211_hw *dev;
+	struct xradio_common *hw_priv;
+	unsigned char randomaddr[ETH_ALEN];
+	const unsigned char *addr = NULL;
+
+	//init xradio_common
+	dev = xradio_init_common(sizeof(struct xradio_common));
+	if (!dev) {
+		dev_dbg(&func->dev, "xradio_init_common failed\n");
+		return err;
+	}
+	hw_priv = dev->priv;
+	hw_priv->pdev = &func->dev;
+	hw_priv->sdio_func = func;
+	sdio_set_drvdata(func, hw_priv);
+
+	// fill in mac addresses
+	if (hw_priv->pdev->of_node) {
+		addr = of_get_mac_address(hw_priv->pdev->of_node);
+	}
+	if (!addr) {
+		dev_warn(hw_priv->pdev, "no mac address provided, using random\n");
+		eth_random_addr(randomaddr);
+		addr = randomaddr;
+	}
+	memcpy(hw_priv->addresses[0].addr, addr, ETH_ALEN);
+	memcpy(hw_priv->addresses[1].addr, addr, ETH_ALEN);
+	hw_priv->addresses[1].addr[5] += 0x01;
+
+	/*init pm and wakelock. */
+#ifdef CONFIG_PM
+	err = xradio_pm_init(&hw_priv->pm_state, hw_priv);
+	if (err) {
+		dev_dbg(hw_priv->pdev, "xradio_pm_init failed(%d).\n", err);
+		goto err2;
+	}
+#endif
+	/* Register bh thread*/
+	err = xradio_register_bh(hw_priv);
+	if (err) {
+		dev_dbg(hw_priv->pdev, "xradio_register_bh failed(%d).\n", err);
+		goto err3;
+	}
+
+	/* Load firmware and register Interrupt Handler */
+	err = xradio_load_firmware(hw_priv);
+	if (err) {
+		dev_dbg(hw_priv->pdev, "xradio_load_firmware failed(%d).\n", err);
+		goto err4;
+	}
+
+	/* Set sdio blocksize. */
+	sdio_lock(hw_priv);
+	WARN_ON(sdio_set_blk_size(hw_priv,
+			SDIO_BLOCK_SIZE));
+	sdio_unlock(hw_priv);
+
+	if (wait_event_interruptible_timeout(hw_priv->wsm_startup_done,
+				hw_priv->wsm_caps.firmwareReady, 3*HZ) <= 0) {
+
+		/* TODO: Needs to find how to reset device */
+		/*       in QUEUE mode properly.           */
+		dev_dbg(hw_priv->pdev, "Firmware Startup Timeout!\n");
+		err = -ETIMEDOUT;
+		goto err5;
+	}
+	dev_dbg(hw_priv->pdev, "Firmware Startup Done.\n");
+
+	/* Keep device wake up. */
+	WARN_ON(xradio_reg_write_16(hw_priv, HIF_CONTROL_REG_ID, HIF_CTRL_WUP_BIT));
+	if (xradio_reg_read_16(hw_priv,HIF_CONTROL_REG_ID, &ctrl_reg))
+		WARN_ON(xradio_reg_read_16(hw_priv,HIF_CONTROL_REG_ID, &ctrl_reg));
+	WARN_ON(!(ctrl_reg & HIF_CTRL_RDY_BIT));
+
+	/* Set device mode parameter. */
+	for (if_id = 0; if_id < xrwl_get_nr_hw_ifaces(hw_priv); if_id++) {
+		/* Set low-power mode. */
+		WARN_ON(wsm_set_operational_mode(hw_priv, &defaultoperationalmode, if_id));
+		/* Enable multi-TX confirmation */
+		WARN_ON(wsm_use_multi_tx_conf(hw_priv, true, if_id));
+	}
+
+	/* Register wireless net device. */
+	err = xradio_register_common(dev);
+	if (err) {
+		dev_dbg(hw_priv->pdev, "xradio_register_common failed(%d)!\n", err);
+		goto err5;
+	}
+
+	return err;
+
+err5:
+	xradio_dev_deinit(hw_priv);
+err4:
+	xradio_unregister_bh(hw_priv);
+err3:
+	xradio_pm_deinit(&hw_priv->pm_state);
+err2:
+/* err1: 	MRK: unused label*/
+	xradio_free_common(dev);
+	return err;
+}
+
+void xradio_core_deinit(struct sdio_func* func)
+{
+	struct xradio_common* hw_priv = sdio_get_drvdata(func);
+	if (hw_priv) {
+		xradio_unregister_common(hw_priv->hw);
+		xradio_dev_deinit(hw_priv);
+		xradio_unregister_bh(hw_priv);
+		xradio_pm_deinit(&hw_priv->pm_state);
+		xradio_free_common(hw_priv->hw);
+	}
+	return;
+}
diff --git a/drivers/net/wireless/xradio/main.h b/drivers/net/wireless/xradio/main.h
new file mode 100644
index 0000000..2238d75
--- /dev/null
+++ b/drivers/net/wireless/xradio/main.h
@@ -0,0 +1,7 @@
+#ifndef __XRADIO_MAIN_H
+#define __XRADIO_MAIN_H
+
+int xradio_core_init(struct sdio_func* func);
+void xradio_core_deinit(struct sdio_func* func);
+
+#endif
\ No newline at end of file
diff --git a/drivers/net/wireless/xradio/module.c b/drivers/net/wireless/xradio/module.c
new file mode 100644
index 0000000..2a41c75
--- /dev/null
+++ b/drivers/net/wireless/xradio/module.c
@@ -0,0 +1,27 @@
+#include <linux/module.h>
+
+#include "xradio.h"
+#include "debug.h"
+#include "sdio.h"
+
+MODULE_AUTHOR("XRadioTech");
+MODULE_DESCRIPTION("XRadioTech WLAN driver core");
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("xradio_core");
+
+/* Init Module function -> Called by insmod */
+static int __init xradio_core_entry(void)
+{
+	int ret = 0;
+	ret = xradio_sdio_register();
+	return ret;
+}
+
+/* Called at Driver Unloading */
+static void __exit xradio_core_exit(void)
+{
+	xradio_sdio_unregister();
+}
+
+module_init(xradio_core_entry);
+module_exit(xradio_core_exit);
diff --git a/drivers/net/wireless/xradio/p2p.c b/drivers/net/wireless/xradio/p2p.c
new file mode 100644
index 0000000..f32fb43
--- /dev/null
+++ b/drivers/net/wireless/xradio/p2p.c
@@ -0,0 +1,62 @@
+#include "xradio.h"
+
+#ifdef TES_P2P_0002_ROC_RESTART
+///w, TES_P2P_0002 WorkAround:
+///w, P2P GO Neg Process and P2P FIND may be collision.
+///w, When P2P Device is waiting for GO NEG CFM in 30ms,
+///w, P2P FIND may end with p2p listen, and then goes to p2p search.
+///w, Then xradio scan will occupy phy on other channel in 3+ seconds.
+///w, P2P Device will not be able to receive the GO NEG CFM.
+///w, We extend the roc period to remaind phy to receive GO NEG CFM as WorkAround.
+
+s32  TES_P2P_0002_roc_dur;
+s32  TES_P2P_0002_roc_sec;
+s32  TES_P2P_0002_roc_usec;
+u32  TES_P2P_0002_packet_id;
+u32  TES_P2P_0002_state =  TES_P2P_0002_STATE_IDLE;
+
+void xradio_frame_monitor(struct xradio_common *hw_priv, struct sk_buff *skb, bool tx) {
+	struct ieee80211_hdr *frame = (struct ieee80211_hdr *)skb->data;
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
+
+	u8 *action        = (u8*)&mgmt->u.action.category;
+	u8 *category_code = &(action[0]);
+	u8 *action_code   = &(action[1]);
+	u8 *oui			  = &(action[2]);
+	u8 *subtype       = &(action[5]);
+	u8 *oui_subtype   = &(action[6]);
+
+
+	if(ieee80211_is_action(frame->frame_control)) {
+		if( *category_code == WLAN_CATEGORY_PUBLIC) {
+			if (*action_code == 0x09) {
+				if((oui[0] == 0x50) && (oui[1] == 0x6F) &&
+				   (oui[2] == 0x9A) && (*subtype == 0x09)) {
+					if ( *oui_subtype == 0x01 ) {  ///w, GO Negotiation Response
+						if((TES_P2P_0002_state == TES_P2P_0002_STATE_IDLE) &&
+						   (tx == true)) { ///w, p2p atturbute:status,id=0
+							u8 *go_neg_resp_res = &(action[17]);
+							if (*go_neg_resp_res == 0x0) {
+								TES_P2P_0002_state = TES_P2P_0002_STATE_SEND_RESP;
+								txrx_printk(XRADIO_DBG_WARN, "[ROC_RESTART_STATE_SEND_RESP]\n");
+							}
+						}
+					} else if ( *oui_subtype == 0x02 ) { ///w, GO Negotiation Confirmation
+						if( tx == false ) {
+							TES_P2P_0002_state = TES_P2P_0002_STATE_IDLE;
+							txrx_printk(XRADIO_DBG_WARN, "[ROC_RESTART_STATE_IDLE]"
+							            "[GO Negotiation Confirmation]\n");
+						}
+					} else if ( *oui_subtype == 0x08 ) { ///w, Provision Discovery Response
+						if(tx == false) {
+							TES_P2P_0002_state = TES_P2P_0002_STATE_IDLE;
+							txrx_printk(XRADIO_DBG_WARN, "[ROC_RESTART_STATE_IDLE]"
+							            "[Provision Discovery Response]\n");
+						}
+					}
+				}
+			}
+		}
+	}
+}
+#endif
\ No newline at end of file
diff --git a/drivers/net/wireless/xradio/p2p.h b/drivers/net/wireless/xradio/p2p.h
new file mode 100644
index 0000000..d86686b
--- /dev/null
+++ b/drivers/net/wireless/xradio/p2p.h
@@ -0,0 +1,6 @@
+#ifndef XRADIO_P2P_H
+#define XRADIO_P2P_H
+
+void xradio_frame_monitor(struct xradio_common *hw_priv, struct sk_buff *skb, bool tx);
+
+#endif
diff --git a/drivers/net/wireless/xradio/pm.c b/drivers/net/wireless/xradio/pm.c
new file mode 100644
index 0000000..488c26c
--- /dev/null
+++ b/drivers/net/wireless/xradio/pm.c
@@ -0,0 +1,800 @@
+/*
+ * PM implementation for XRadio drivers
+ *
+ * Copyright (c) 2013, XRadio
+ * Author: XRadio
+ *
+ * This program is free software; you can 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/platform_device.h>
+#include <linux/if_ether.h>
+#include "xradio.h"
+#include "pm.h"
+#include "sta.h"
+#include "bh.h"
+#include "sdio.h"
+
+#define XRADIO_BEACON_SKIPPING_MULTIPLIER 3
+
+struct xradio_udp_port_filter {
+	struct wsm_udp_port_filter_hdr hdr;
+	struct wsm_udp_port_filter dhcp;
+	struct wsm_udp_port_filter upnp;
+} __packed;
+
+struct xradio_ether_type_filter {
+	struct wsm_ether_type_filter_hdr hdr;
+	struct wsm_ether_type_filter ip;
+	struct wsm_ether_type_filter pae;
+	struct wsm_ether_type_filter wapi;
+} __packed;
+
+static struct xradio_udp_port_filter xradio_udp_port_filter_on = {
+	.hdr.nrFilters = 2,
+	.dhcp = {
+		.filterAction = WSM_FILTER_ACTION_FILTER_OUT,
+		.portType = WSM_FILTER_PORT_TYPE_DST,
+		.udpPort = __cpu_to_le16(67),
+	},
+	.upnp = {
+		.filterAction = WSM_FILTER_ACTION_FILTER_OUT,
+		.portType = WSM_FILTER_PORT_TYPE_DST,
+		.udpPort = __cpu_to_le16(1900),
+	},
+	/* Please add other known ports to be filtered out here and
+	 * update nrFilters field in the header.
+	 * Up to 4 filters are allowed. */
+};
+
+static struct wsm_udp_port_filter_hdr xradio_udp_port_filter_off = {
+	.nrFilters = 0,
+};
+
+#ifndef ETH_P_WAPI
+#define ETH_P_WAPI     0x88B4
+#endif
+
+#ifdef TES_P2P_000B_DISABLE_EAPOL_FILTER
+/* TES_P2P_000B WorkAround:
+ * when the link keep 10min more or less(i am not sure),
+ * wpa_s session maybe expired, and want to update group key.
+ * it will use eapol frame(802.1x,0x888E).
+ * if driver suspend, and discard eapol frame, then session end.
+ * i don't know why original code discards eapol frame in suspend.
+ * but now make this filter disable as WorkAround. wzw */
+static struct xradio_ether_type_filter xradio_ether_type_filter_on = {
+	.hdr.nrFilters = 1,
+/*	.ip = {
+		.filterAction = WSM_FILTER_ACTION_FILTER_IN,
+		.etherType = __cpu_to_le16(ETH_P_IP),
+	},*/
+/*	.pae = {
+		.filterAction = WSM_FILTER_ACTION_FILTER_IN,
+		.etherType = __cpu_to_le16(ETH_P_PAE),
+	},*/
+	.wapi = {
+		.filterAction = WSM_FILTER_ACTION_FILTER_IN,
+		.etherType = __cpu_to_le16(ETH_P_WAPI),
+	},
+	/* Please add other known ether types to be filtered out here and
+	 * update nrFilters field in the header.
+	 * Up to 4 filters are allowed. */
+};
+#else
+static struct xradio_ether_type_filter xradio_ether_type_filter_on = {
+	.hdr.nrFilters = 2,
+/*	.ip = {
+		.filterAction = WSM_FILTER_ACTION_FILTER_IN,
+		.etherType = __cpu_to_le16(ETH_P_IP),
+	},*/
+	.pae = {
+		.filterAction = WSM_FILTER_ACTION_FILTER_IN,
+		.etherType = __cpu_to_le16(ETH_P_PAE),
+	},
+	.wapi = {
+		.filterAction = WSM_FILTER_ACTION_FILTER_IN,
+		.etherType = __cpu_to_le16(ETH_P_WAPI),
+	},
+	/* Please add other known ether types to be filtered out here and
+	 * update nrFilters field in the header.
+	 * Up to 4 filters are allowed. */
+};
+#endif
+
+static struct wsm_ether_type_filter_hdr xradio_ether_type_filter_off = {
+	.nrFilters = 0,
+};
+
+static int xradio_suspend_late(struct device *dev);
+static void xradio_pm_release(struct device *dev);
+static int xradio_pm_probe(struct platform_device *pdev);
+static int __xradio_wow_suspend(struct xradio_vif *priv,
+                                struct cfg80211_wowlan *wowlan);
+static int __xradio_wow_resume(struct xradio_vif *priv);
+#ifdef CONFIG_XRADIO_SUSPEND_POWER_OFF
+static int xradio_poweroff_suspend(struct xradio_common *hw_priv);
+static int xradio_poweroff_resume(struct xradio_common *hw_priv);
+#endif
+
+
+/* private */
+struct xradio_suspend_state {
+	unsigned long bss_loss_tmo;
+	unsigned long connection_loss_tmo;
+	unsigned long join_tmo;
+	unsigned long direct_probe;
+	unsigned long link_id_gc;
+	bool beacon_skipping;
+};
+
+static const struct dev_pm_ops xradio_pm_ops = {
+	.suspend_noirq = xradio_suspend_late,
+};
+
+static struct platform_driver xradio_power_driver = {
+	.probe = xradio_pm_probe,
+	.driver = {
+		.name = XRADIO_PM_DEVICE,
+		.pm = &xradio_pm_ops,
+	},
+};
+
+static int xradio_pm_init_common(struct xradio_pm_state *pm,
+				  struct xradio_common *hw_priv)
+{
+	int ret;
+	pm_printk(XRADIO_DBG_TRC,"%s\n", __FUNCTION__);
+
+	spin_lock_init(&pm->lock);
+	/* Register pm driver. */
+	ret = platform_driver_register(&xradio_power_driver);
+	if (ret) {
+		pm_printk(XRADIO_DBG_ERROR, "%s:platform_driver_register failed(%d)!\n",
+		           __FUNCTION__, ret);
+		return ret;
+	}
+
+	/* Add pm device. */
+	pm->pm_dev = platform_device_alloc(XRADIO_PM_DEVICE, 0);
+	if (!pm->pm_dev) {
+		pm_printk(XRADIO_DBG_ERROR, "%s:platform_device_alloc failed!\n",
+		           __FUNCTION__);
+		platform_driver_unregister(&xradio_power_driver);
+		return -ENOMEM;
+	}
+	pm->pm_dev->dev.platform_data = hw_priv;
+	ret = platform_device_add(pm->pm_dev);
+	if (ret) {
+		pm_printk(XRADIO_DBG_ERROR, "%s:platform_device_add failed(%d)!\n",
+		           __FUNCTION__, ret);
+		platform_driver_unregister(&xradio_power_driver);
+		kfree(pm->pm_dev);
+		pm->pm_dev = NULL;
+	}
+
+	return ret;
+}
+
+static void xradio_pm_deinit_common(struct xradio_pm_state *pm)
+{
+	pm_printk(XRADIO_DBG_TRC,"%s\n", __FUNCTION__);
+	platform_driver_unregister(&xradio_power_driver);
+	if (pm->pm_dev) {
+		pm->pm_dev->dev.platform_data = NULL;
+		platform_device_unregister(pm->pm_dev); /* kfree is already do */
+		pm->pm_dev = NULL;
+	}
+}
+
+#ifdef CONFIG_WAKELOCK
+
+int xradio_pm_init(struct xradio_pm_state *pm,
+		   struct xradio_common *hw_priv)
+{
+	int ret = 0;
+	pm_printk(XRADIO_DBG_TRC,"%s\n", __FUNCTION__);
+
+	ret = xradio_pm_init_common(pm, hw_priv);
+	if (!ret)
+		wake_lock_init(&pm->wakelock, WAKE_LOCK_SUSPEND, XRADIO_WAKE_LOCK);
+	else
+		pm_printk(XRADIO_DBG_ERROR,"xradio_pm_init_common failed!\n");
+	return ret;
+}
+
+void xradio_pm_deinit(struct xradio_pm_state *pm)
+{
+	pm_printk(XRADIO_DBG_TRC,"%s\n", __FUNCTION__);
+	if (wake_lock_active(&pm->wakelock))
+		wake_unlock(&pm->wakelock);
+	wake_lock_destroy(&pm->wakelock);
+	xradio_pm_deinit_common(pm);
+}
+
+void xradio_pm_stay_awake(struct xradio_pm_state *pm,
+			  unsigned long tmo)
+{
+	long cur_tmo;
+	pm_printk(XRADIO_DBG_MSG,"%s\n", __FUNCTION__);
+
+	spin_lock_bh(&pm->lock);
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 10, 0))
+	cur_tmo = pm->wakelock.ws.timer.expires - jiffies;
+#else
+	cur_tmo = pm->wakelock.expires - jiffies;
+#endif
+	if (!wake_lock_active(&pm->wakelock) || cur_tmo < (long)tmo)
+		wake_lock_timeout(&pm->wakelock, tmo);
+	spin_unlock_bh(&pm->lock);
+}
+void xradio_pm_lock_awake(struct xradio_pm_state *pm)
+{
+
+	spin_lock_bh(&pm->lock);
+#if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 10, 0))
+	pm->expires_save = pm->wakelock.ws.timer.expires;
+#else
+	pm->expires_save = pm->wakelock.expires;
+#endif
+	wake_lock_timeout(&pm->wakelock, LONG_MAX);
+	spin_unlock_bh(&pm->lock);
+}
+void xradio_pm_unlock_awake(struct xradio_pm_state *pm)
+{
+
+	spin_lock_bh(&pm->lock);
+	pm->expires_save -= jiffies;
+	if (pm->expires_save)
+		wake_lock_timeout(&pm->wakelock, pm->expires_save);
+	else
+		wake_lock_timeout(&pm->wakelock, 1);
+	spin_unlock_bh(&pm->lock);
+}
+
+#else /* CONFIG_WAKELOCK */
+
+static void xradio_pm_stay_awake_tmo(struct timer_list *t)
+{
+	struct xradio_pm_state *pm = from_timer(pm, t, stay_awake);
+	(void)pm;
+}
+
+int xradio_pm_init(struct xradio_pm_state *pm,
+		   struct xradio_common *hw_priv)
+{
+	int ret = 0;
+	pm_printk(XRADIO_DBG_MSG,"%s\n", __FUNCTION__);
+
+	ret = xradio_pm_init_common(pm, hw_priv);
+	if (!ret) {
+		timer_setup(&pm->stay_awake, xradio_pm_stay_awake_tmo, 0);
+	} else 
+		pm_printk(XRADIO_DBG_ERROR,"xradio_pm_init_common failed!\n");
+	return ret;
+}
+
+void xradio_pm_deinit(struct xradio_pm_state *pm)
+{
+	del_timer_sync(&pm->stay_awake);
+	xradio_pm_deinit_common(pm);
+}
+
+void xradio_pm_stay_awake(struct xradio_pm_state *pm,
+			  unsigned long tmo)
+{
+	long cur_tmo;
+
+	spin_lock_bh(&pm->lock);
+	cur_tmo = pm->stay_awake.expires - jiffies;
+	if (!timer_pending(&pm->stay_awake) || cur_tmo < (long)tmo)
+		mod_timer(&pm->stay_awake, jiffies + tmo);
+	spin_unlock_bh(&pm->lock);
+}
+void xradio_pm_lock_awake(struct xradio_pm_state *pm)
+{
+
+	spin_lock_bh(&pm->lock);
+	pm->expires_save = pm->stay_awake.expires;
+	mod_timer(&pm->stay_awake, jiffies + LONG_MAX);
+	spin_unlock_bh(&pm->lock);
+}
+void xradio_pm_unlock_awake(struct xradio_pm_state *pm)
+{
+
+	spin_lock_bh(&pm->lock);
+	if (time_before(jiffies, pm->expires_save))
+		mod_timer(&pm->stay_awake, pm->expires_save);
+	else
+		mod_timer(&pm->stay_awake, jiffies + 1);
+	spin_unlock_bh(&pm->lock);
+}
+#endif /* CONFIG_WAKELOCK */
+
+static long xradio_suspend_work(struct delayed_work *work)
+{
+	int ret = cancel_delayed_work(work);
+	long tmo;
+	pm_printk(XRADIO_DBG_TRC, "%s\n", __func__);
+
+	if (ret > 0) {
+		/* Timer is pending */
+		tmo = work->timer.expires - jiffies;
+		if (tmo < 0)
+			tmo = 0;
+	} else {
+		tmo = -1;
+	}
+	return tmo;
+}
+
+static int xradio_resume_work(struct xradio_common *hw_priv,
+			       struct delayed_work *work,
+			       unsigned long tmo)
+{
+	pm_printk(XRADIO_DBG_TRC, "%s\n", __func__);
+	if ((long)tmo < 0)
+		return 1;
+
+	return queue_delayed_work(hw_priv->workqueue, work, tmo);
+}
+
+static int xradio_suspend_late(struct device *dev)
+{
+#ifdef CONFIG_XRADIO_SUSPEND_POWER_OFF
+	struct xradio_common *hw_priv = dev->platform_data;
+
+	if (XRADIO_POWEROFF_SUSP == atomic_read(&hw_priv->suspend_state)) {
+		return 0; /* we don't rx data when power down wifi.*/
+	}
+#endif
+
+	//if (atomic_read(&hw_priv->bh_rx)) {
+	//	pm_printk(XRADIO_DBG_WARN, "%s: Suspend interrupted.\n", __func__);
+	//	return -EAGAIN;
+	//}
+	return 0;
+}
+
+static void xradio_pm_release(struct device *dev)
+{
+	pm_printk(XRADIO_DBG_TRC, "%s\n", __func__);
+}
+
+static int xradio_pm_probe(struct platform_device *pdev)
+{
+	pm_printk(XRADIO_DBG_TRC, "%s\n", __func__);
+	pdev->dev.release = xradio_pm_release;
+	return 0;
+}
+
+int xradio_wow_suspend(struct ieee80211_hw *hw, struct cfg80211_wowlan *wowlan)
+{
+	struct xradio_common *hw_priv = hw->priv;
+	struct xradio_vif *priv;
+	int i, ret = 0;
+
+
+	if(hw_priv->bh_error) return -EBUSY;
+	WARN_ON(!atomic_read(&hw_priv->num_vifs));
+
+	if (work_pending(&hw_priv->query_work))
+		return -EBUSY;
+
+#ifdef ROAM_OFFLOAD
+	xradio_for_each_vif(hw_priv, priv, i) {
+		if (!priv)
+			continue;
+		if((priv->vif->type == NL80211_IFTYPE_STATION)
+		&& (priv->join_status == XRADIO_JOIN_STATUS_STA)) {
+			down(&hw_priv->scan.lock);
+			hw_priv->scan.if_id = priv->if_id;
+			xradio_sched_scan_work(&hw_priv->scan.swork);
+		}
+	}
+#endif /*ROAM_OFFLOAD*/
+
+	/* Do not suspend when datapath is not idle */
+	if (hw_priv->tx_queue_stats.num_queued[0] + 
+		  hw_priv->tx_queue_stats.num_queued[1]) {
+		pm_printk(XRADIO_DBG_WARN, "Don't suspend "
+		           "because of tx_queue is not empty.\n");
+		return -EBUSY;
+	}
+
+	/* Make sure there is no configuration requests in progress. */
+	if (!mutex_trylock(&hw_priv->conf_mutex)) {
+		pm_printk(XRADIO_DBG_WARN, "Don't suspend "
+		           "because of configuration requests.\n");
+		return -EBUSY;
+	}
+
+	/* Make sure there is no wsm_oper_lock in progress. */
+	if (!mutex_trylock(&hw_priv->wsm_oper_lock)) {
+		pm_printk(XRADIO_DBG_WARN, "Don't suspend "
+			   "because of wsm_oper_lock.\n");
+		mutex_unlock(&hw_priv->conf_mutex);
+		return -EBUSY;
+	}
+
+	/* Do not suspend when scanning or ROC*/
+	if (down_trylock(&hw_priv->scan.lock)) {
+		pm_printk(XRADIO_DBG_WARN, "Don't suspend "
+		           "because of scan requests.\n");
+		goto revert1;
+	}
+
+	if (delayed_work_pending(&hw_priv->scan.probe_work)) {
+		pm_printk(XRADIO_DBG_WARN, "Don't suspend "
+		           "because of probe frames tx in progress.\n");
+		goto revert2;
+	}
+
+	/* Lock TX. */
+	wsm_lock_tx_async(hw_priv);
+
+	/* Wait to avoid possible race with bh code.
+	 * But do not wait too long... */
+	if (wait_event_timeout(hw_priv->bh_evt_wq, 
+	                       !hw_priv->hw_bufs_used, HZ / 10) <= 0) {
+		pm_printk(XRADIO_DBG_WARN, "Don't suspend "
+		           "because of there are frames not confirm.\n");
+		goto revert3;
+	}
+		
+#ifdef CONFIG_XRADIO_SUSPEND_POWER_OFF
+//	if (STANDBY_WITH_POWER_OFF == standby_level) {
+	if (1) {
+		return xradio_poweroff_suspend(hw_priv);
+	}
+#endif
+	
+	xradio_for_each_vif(hw_priv, priv, i) {
+		if (!priv)
+			continue;
+
+		ret = __xradio_wow_suspend(priv, wowlan);
+		if (ret) {
+			for (; i >= 0; i--) {
+				if (!hw_priv->vif_list[i])
+					continue;
+				priv = (struct xradio_vif *)hw_priv->vif_list[i]->drv_priv;
+				__xradio_wow_resume(priv);
+			}
+			pm_printk(XRADIO_DBG_WARN, "Don't suspend "
+			           "because of __xradio_wow_suspend failed!\n");
+			goto revert3;
+		}
+	}
+
+	/* Stop serving thread */
+	if (xradio_bh_suspend(hw_priv)) {
+		pm_printk(XRADIO_DBG_WARN, "Don't suspend "
+		           "because of xradio_bh_suspend failed!\n");
+		xradio_wow_resume(hw);
+		return -EBUSY;
+	}
+
+	/* Enable IRQ wake */
+	ret = sdio_pm(hw_priv, true);
+	if (ret) {
+		pm_printk(XRADIO_DBG_WARN, "Don't suspend sbus pm failed\n");
+		xradio_wow_resume(hw);
+		return -EBUSY;
+	}
+
+	/* Force resume if event is coming from the device. */
+	//if (atomic_read(&hw_priv->bh_rx)) {
+	//	pm_printk(XRADIO_DBG_WARN, "Don't suspend "
+	//	           "because of recieved rx event!\n");
+	//	xradio_wow_resume(hw);
+	//	return -EAGAIN;
+	//}
+	return 0;
+
+revert3:
+	wsm_unlock_tx(hw_priv);
+revert2:
+	up(&hw_priv->scan.lock);
+revert1:
+	mutex_unlock(&hw_priv->conf_mutex);
+	mutex_unlock(&hw_priv->wsm_oper_lock);
+	return -EBUSY;
+}
+
+static int __xradio_wow_suspend(struct xradio_vif *priv,
+				struct cfg80211_wowlan *wowlan)
+{
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	struct xradio_pm_state_vif *pm_state_vif = &priv->pm_state_vif;
+	struct xradio_suspend_state *state;
+	int ret;
+#ifdef MCAST_FWDING
+	struct wsm_forwarding_offload fwdoffload = {
+		.fwenable = 0x1,
+		.flags    = 0x1,
+	};
+#endif
+
+
+	/* Do not suspend when join work is scheduled */
+	if (work_pending(&priv->join_work)) {
+		pm_printk(XRADIO_DBG_WARN, "%s:Do not suspend "
+		           "when join work is scheduled\n", __func__);
+		goto revert1;
+	}
+
+	/* Set UDP filter */
+	wsm_set_udp_port_filter(hw_priv, &xradio_udp_port_filter_on.hdr,
+	                        priv->if_id);
+
+	/* Set ethernet frame type filter */
+	wsm_set_ether_type_filter(hw_priv, &xradio_ether_type_filter_on.hdr,
+	                          priv->if_id);
+
+	/* Set IP multicast filter */
+    wsm_set_host_sleep(hw_priv, 1, priv->if_id);
+    
+	if (priv->join_status == XRADIO_JOIN_STATUS_AP)
+		WARN_ON(wsm_set_keepalive_filter(priv, true));
+
+#ifdef XRADIO_SUSPEND_RESUME_FILTER_ENABLE
+	/* Set Multicast Address Filter */
+	if (priv->multicast_filter.numOfAddresses) {
+		priv->multicast_filter.enable = 1;
+		wsm_set_multicast_filter(hw_priv, &priv->multicast_filter, priv->if_id);
+	}
+
+	/* Set Enable Broadcast Address Filter */
+	priv->broadcast_filter.action_mode = 1;
+	if (priv->join_status == XRADIO_JOIN_STATUS_AP)
+		priv->broadcast_filter.address_mode = 3;
+
+	xradio_set_macaddrfilter(hw_priv, priv, (u8 *)&priv->broadcast_filter);
+#endif
+
+#ifdef MCAST_FWDING
+	if (priv->join_status == XRADIO_JOIN_STATUS_AP)
+		WARN_ON(wsm_set_forwarding_offlad(hw_priv, &fwdoffload,priv->if_id));
+#endif
+
+	/* Allocate state */
+	state = kzalloc(sizeof(struct xradio_suspend_state), GFP_KERNEL);
+	if (!state) {
+		pm_printk(XRADIO_DBG_WARN, "%s:Do not suspend "
+		           "alloc xradio_suspend_state failed.\n", __func__);
+		goto revert2;
+	}
+	/* Store delayed work states. */
+	state->bss_loss_tmo        = xradio_suspend_work(&priv->bss_loss_work);
+	state->connection_loss_tmo = xradio_suspend_work(&priv->connection_loss_work);
+	state->join_tmo   = xradio_suspend_work(&priv->join_timeout);
+	state->link_id_gc = xradio_suspend_work(&priv->link_id_gc_work);
+
+	/* Enable beacon skipping */
+	if (priv->join_status == XRADIO_JOIN_STATUS_STA && 
+		  priv->join_dtim_period &&  !priv->has_multicast_subscription) {
+		state->beacon_skipping = true;
+		wsm_set_beacon_wakeup_period(hw_priv, priv->join_dtim_period,
+		                             XRADIO_BEACON_SKIPPING_MULTIPLIER * \
+		                             priv->join_dtim_period, priv->if_id);
+	}
+
+	ret = timer_pending(&priv->mcast_timeout);
+	if (ret) {
+		pm_printk(XRADIO_DBG_WARN, "%s:Do not suspend "
+		           "mcast timeout timer_pending failed.\n", __func__);
+		goto revert3;
+	}
+
+	/* Store suspend state */
+	pm_state_vif->suspend_state = state;
+
+	return 0;
+
+revert3:
+	xradio_resume_work(hw_priv, &priv->bss_loss_work, state->bss_loss_tmo);
+	xradio_resume_work(hw_priv, &priv->connection_loss_work, 
+	                   state->connection_loss_tmo);
+	xradio_resume_work(hw_priv, &priv->join_timeout, state->join_tmo);
+	xradio_resume_work(hw_priv, &priv->link_id_gc_work, state->link_id_gc);
+	kfree(state);
+
+revert2:
+	wsm_set_udp_port_filter(hw_priv, &xradio_udp_port_filter_off, priv->if_id);
+	wsm_set_ether_type_filter(hw_priv, &xradio_ether_type_filter_off, priv->if_id);
+    wsm_set_host_sleep(hw_priv, 0, priv->if_id);
+
+	if (priv->join_status == XRADIO_JOIN_STATUS_AP)
+		WARN_ON(wsm_set_keepalive_filter(priv, false));
+
+#ifdef XRADIO_SUSPEND_RESUME_FILTER_ENABLE
+	/* Set Multicast Address Filter */
+	if (priv->multicast_filter.numOfAddresses) {
+		priv->multicast_filter.enable = 0;
+		wsm_set_multicast_filter(hw_priv, &priv->multicast_filter, priv->if_id);
+	}
+
+	/* Set Enable Broadcast Address Filter */
+	priv->broadcast_filter.action_mode = 0;
+	if (priv->join_status == XRADIO_JOIN_STATUS_AP)
+		priv->broadcast_filter.address_mode = 0;
+	xradio_set_macaddrfilter(hw_priv, priv, (u8 *)&priv->broadcast_filter);
+#endif
+
+#ifdef MCAST_FWDING
+	fwdoffload.flags = 0x0;	
+	if (priv->join_status == XRADIO_JOIN_STATUS_AP)
+		WARN_ON(wsm_set_forwarding_offlad(hw_priv, &fwdoffload,priv->if_id));
+#endif
+
+revert1:
+	/* mutex_unlock(&hw_priv->conf_mutex); */
+	return -EBUSY;
+}
+
+int xradio_wow_resume(struct ieee80211_hw *hw)
+{
+
+	struct xradio_common *hw_priv = hw->priv;
+	struct xradio_vif *priv;
+	int i, ret = 0;
+
+
+	WARN_ON(!atomic_read(&hw_priv->num_vifs));
+	if(hw_priv->bh_error) return 0;
+
+#ifdef CONFIG_XRADIO_SUSPEND_POWER_OFF
+	if (XRADIO_POWEROFF_SUSP == atomic_read(&hw_priv->suspend_state)) {
+		return xradio_poweroff_resume(hw_priv);
+	}
+#endif
+
+	/* Disable IRQ wake */
+	sdio_pm(hw_priv, false);
+
+	up(&hw_priv->scan.lock);
+
+	/* Resume BH thread */
+	WARN_ON(xradio_bh_resume(hw_priv));
+
+	xradio_for_each_vif(hw_priv, priv, i) {
+		if (!priv)
+			continue;
+		ret = __xradio_wow_resume(priv);
+		if (ret) {
+			pm_printk(XRADIO_DBG_ERROR, "%s:__xradio_wow_resume failed!\n", __func__);
+			break;
+		}
+	}
+
+	wsm_unlock_tx(hw_priv);
+
+	/* Unlock configuration mutex */
+	mutex_unlock(&hw_priv->conf_mutex);
+	mutex_unlock(&hw_priv->wsm_oper_lock);
+
+	return ret;
+}
+
+static int __xradio_wow_resume(struct xradio_vif *priv)
+{
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	struct xradio_pm_state_vif *pm_state_vif = &priv->pm_state_vif;
+	struct xradio_suspend_state *state;
+#ifdef MCAST_FWDING
+	struct wsm_forwarding_offload fwdoffload = {
+		.fwenable = 0x1,
+		.flags = 0x0,
+	};
+#endif
+
+
+	/* Restore suspend state */
+	state = pm_state_vif->suspend_state;
+	pm_state_vif->suspend_state = NULL;
+
+#ifdef ROAM_OFFLOAD
+	if((priv->vif->type == NL80211_IFTYPE_STATION)
+	&& (priv->join_status == XRADIO_JOIN_STATUS_STA))
+		xradio_hw_sched_scan_stop(hw_priv);
+#endif /*ROAM_OFFLOAD*/
+
+	if (state->beacon_skipping) {
+#ifdef XRADIO_USE_LONG_DTIM_PERIOD
+		int join_dtim_period_extend;
+		if (priv->join_dtim_period <= 3) {
+			join_dtim_period_extend = priv->join_dtim_period * 3;
+		} else if (priv->join_dtim_period <= 5) {
+			join_dtim_period_extend = priv->join_dtim_period * 2;
+		} else {
+			join_dtim_period_extend = priv->join_dtim_period;
+		}
+		wsm_set_beacon_wakeup_period(hw_priv,
+			((priv->beacon_int * join_dtim_period_extend) > MAX_BEACON_SKIP_TIME_MS ?
+			 1 : join_dtim_period_extend) , 0, priv->if_id);
+#else
+		wsm_set_beacon_wakeup_period(hw_priv, priv->beacon_int *
+			(priv->join_dtim_period > MAX_BEACON_SKIP_TIME_MS ? 1 : priv->join_dtim_period), 
+			0, priv->if_id);
+#endif
+		state->beacon_skipping = false;
+	}
+
+	if (priv->join_status == XRADIO_JOIN_STATUS_AP)
+		WARN_ON(wsm_set_keepalive_filter(priv, false));
+
+#ifdef XRADIO_SUSPEND_RESUME_FILTER_ENABLE
+	/* Set Multicast Address Filter */
+	if (priv->multicast_filter.numOfAddresses) {
+		priv->multicast_filter.enable = 0;
+		wsm_set_multicast_filter(hw_priv, &priv->multicast_filter, priv->if_id);
+	}
+	/* Set Enable Broadcast Address Filter */
+	priv->broadcast_filter.action_mode = 0;
+	if (priv->join_status == XRADIO_JOIN_STATUS_AP)
+		priv->broadcast_filter.address_mode = 0;
+	xradio_set_macaddrfilter(hw_priv, priv, (u8 *)&priv->broadcast_filter);
+#endif
+
+#ifdef MCAST_FWDING
+	if (priv->join_status == XRADIO_JOIN_STATUS_AP)
+		WARN_ON(wsm_set_forwarding_offlad(hw_priv, &fwdoffload,priv->if_id));
+#endif
+
+	/* Resume delayed work */
+	xradio_resume_work(hw_priv, &priv->bss_loss_work, state->bss_loss_tmo);
+	xradio_resume_work(hw_priv, &priv->connection_loss_work,
+	                   state->connection_loss_tmo);
+	xradio_resume_work(hw_priv, &priv->join_timeout, state->join_tmo);
+	xradio_resume_work(hw_priv, &priv->link_id_gc_work, state->link_id_gc);
+
+	/* Remove UDP port filter */
+	wsm_set_udp_port_filter(hw_priv, &xradio_udp_port_filter_off, priv->if_id);
+
+	/* Remove ethernet frame type filter */
+	wsm_set_ether_type_filter(hw_priv, &xradio_ether_type_filter_off, priv->if_id);
+	
+	/* Remove IP multicast filter */
+    wsm_set_host_sleep(hw_priv, 0, priv->if_id);
+	/* Free memory */
+	kfree(state);
+
+	return 0;
+}
+#ifdef CONFIG_XRADIO_SUSPEND_POWER_OFF
+static int xradio_poweroff_suspend(struct xradio_common *hw_priv)
+{
+
+	//flush all work.
+	cancel_work_sync(&hw_priv->query_work);
+	flush_workqueue(hw_priv->workqueue);
+	/* Schedule hardware restart, ensure no cmds in progress.*/
+	mutex_lock(&hw_priv->wsm_cmd_mux);
+	atomic_set(&hw_priv->suspend_state, XRADIO_POWEROFF_SUSP);
+	//hw_priv->hw_restart = true;
+	mutex_unlock(&hw_priv->wsm_cmd_mux);
+	/* Stop serving thread */
+	if (xradio_bh_suspend(hw_priv)) {
+		pm_printk(XRADIO_DBG_WARN, "%s, xradio_bh_suspend failed!\n", __func__);
+		return -EBUSY;
+	}
+
+	return 0;
+}
+
+static int xradio_poweroff_resume(struct xradio_common *hw_priv)
+{
+
+	/* Revert locks */
+	wsm_unlock_tx(hw_priv);
+	up(&hw_priv->scan.lock);
+	mutex_unlock(&hw_priv->conf_mutex);
+	mutex_unlock(&hw_priv->wsm_oper_lock);
+	//if (schedule_work(&hw_priv->hw_restart_work) <= 0)
+	//	pm_printk(XRADIO_DBG_ERROR, "%s restart_work failed!\n", __func__);
+	return 0;
+}
+#endif
diff --git a/drivers/net/wireless/xradio/pm.h b/drivers/net/wireless/xradio/pm.h
new file mode 100644
index 0000000..6443cbc
--- /dev/null
+++ b/drivers/net/wireless/xradio/pm.h
@@ -0,0 +1,64 @@
+/*
+ * power management interfaces for XRadio drivers
+ *
+ * Copyright (c) 2013, XRadio
+ * Author: XRadio
+ *
+ * This program is free software; you can 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 PM_H_INCLUDED
+#define PM_H_INCLUDED
+
+#ifdef CONFIG_WAKELOCK
+#include <linux/wakelock.h>
+#endif
+
+/* ******************************************************************** */
+/* mac80211 API */
+
+#ifdef CONFIG_PM
+
+#define XRADIO_PM_DEVICE   "xradio_pm"
+#define XRADIO_WAKE_LOCK   "xradio_wlan"
+
+/* extern */   struct xradio_common; 
+ /* private */ struct xradio_suspend_state;
+
+struct xradio_pm_state {
+#ifdef CONFIG_WAKELOCK
+	struct wake_lock wakelock;
+#else
+	struct timer_list stay_awake;
+#endif
+	struct platform_device *pm_dev;
+	spinlock_t lock;
+	unsigned long expires_save;
+};
+
+struct xradio_pm_state_vif {
+	struct xradio_suspend_state *suspend_state;
+};
+
+#ifdef CONFIG_XRADIO_SUSPEND_POWER_OFF
+enum suspend_state {
+	XRADIO_RESUME = 0,
+	XRADIO_CONNECT_SUSP,
+	XRADIO_DISCONNECT_SUSP,
+	XRADIO_POWEROFF_SUSP
+};
+#endif
+int xradio_pm_init(struct xradio_pm_state *pm, struct xradio_common *priv);
+void xradio_pm_deinit(struct xradio_pm_state *pm);
+void xradio_pm_stay_awake(struct xradio_pm_state *pm, unsigned long tmo);
+void xradio_pm_lock_awake(struct xradio_pm_state *pm);
+void xradio_pm_unlock_awake(struct xradio_pm_state *pm);
+int xradio_wow_suspend(struct ieee80211_hw *hw, struct cfg80211_wowlan *wowlan);
+int xradio_wow_resume(struct ieee80211_hw *hw);
+
+#endif /* CONFIG_PM */
+
+#endif
diff --git a/drivers/net/wireless/xradio/queue.c b/drivers/net/wireless/xradio/queue.c
new file mode 100644
index 0000000..e4b00ea
--- /dev/null
+++ b/drivers/net/wireless/xradio/queue.c
@@ -0,0 +1,820 @@
+/*
+ * Queue implementation for XRadio drivers
+ *
+ * Copyright (c) 2013, XRadio
+ * Author: XRadio
+ *
+ * This program is free software; you can 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 <net/mac80211.h>
+#include <linux/sched.h>
+#include "xradio.h"
+#include "queue.h"
+
+/* private */ struct xradio_queue_item
+{
+	struct list_head	head;
+	struct sk_buff		*skb;
+	u32			packetID;
+	unsigned long		queue_timestamp;
+	unsigned long		xmit_timestamp;
+	struct xradio_txpriv	txpriv;
+	u8			generation;
+	u8			pack_stk_wr;
+};
+
+static inline void __xradio_queue_lock(struct xradio_queue *queue)
+{
+	struct xradio_queue_stats *stats = queue->stats;
+	if (queue->tx_locked_cnt++ == 0) {
+		txrx_printk(XRADIO_DBG_MSG, "[TX] Queue %d is locked.\n",
+				queue->queue_id);
+		ieee80211_stop_queue(stats->hw_priv->hw, queue->queue_id);
+	}
+}
+
+static inline void __xradio_queue_unlock(struct xradio_queue *queue)
+{
+	struct xradio_queue_stats *stats = queue->stats;
+	BUG_ON(!queue->tx_locked_cnt);
+	if (--queue->tx_locked_cnt == 0) {
+		txrx_printk(XRADIO_DBG_MSG, "[TX] Queue %d is unlocked.\n",
+				queue->queue_id);
+		ieee80211_wake_queue(stats->hw_priv->hw, queue->queue_id);
+	}
+}
+
+static inline void xradio_queue_parse_id(u32 packetID, u8 *queue_generation,
+						u8 *queue_id,
+						u8 *item_generation,
+						u8 *item_id,
+						u8 *if_id,
+						u8 *link_id)
+{
+	*item_id		= (packetID >>  0) & 0xFF;
+	*item_generation	= (packetID >>  8) & 0xFF;
+	*queue_id		= (packetID >> 16) & 0xF;
+	*if_id			= (packetID >> 20) & 0xF;
+	*link_id		= (packetID >> 24) & 0xF;
+	*queue_generation	= (packetID >> 28) & 0xF;
+}
+
+static inline u32 xradio_queue_make_packet_id(u8 queue_generation, u8 queue_id,
+						u8 item_generation, u8 item_id,
+						u8 if_id, u8 link_id)
+{
+	/*TODO:COMBO: Add interfaceID to the packetID */
+	return ((u32)item_id << 0) |
+		((u32)item_generation << 8) |
+		((u32)queue_id << 16) |
+		((u32)if_id << 20) |
+		((u32)link_id << 24) |
+		((u32)queue_generation << 28);
+}
+
+static void xradio_queue_post_gc(struct xradio_queue_stats *stats,
+				 struct list_head *gc_list)
+{
+	struct xradio_queue_item *item;
+
+	while (!list_empty(gc_list)) {
+		item = list_first_entry(
+			gc_list, struct xradio_queue_item, head);
+		list_del(&item->head);
+		stats->skb_dtor(stats->hw_priv, item->skb, &item->txpriv);
+		kfree(item);
+	}
+}
+
+static void xradio_queue_register_post_gc(struct list_head *gc_list,
+				     struct xradio_queue_item *item)
+{
+	struct xradio_queue_item *gc_item;
+	gc_item = kmalloc(sizeof(struct xradio_queue_item), GFP_KERNEL);
+	BUG_ON(!gc_item);
+	memcpy(gc_item, item, sizeof(struct xradio_queue_item));
+	list_add_tail(&gc_item->head, gc_list);
+}
+
+static void __xradio_queue_gc(struct xradio_queue *queue,
+			      struct list_head *head,
+			      bool unlock)
+{
+	struct xradio_queue_stats *stats = queue->stats;
+	struct xradio_queue_item *item = NULL;
+	//struct xradio_vif *priv;
+	int if_id;
+	bool wakeup_stats = false;
+
+	while (!list_empty(&queue->queue)) {
+		struct xradio_txpriv *txpriv;
+		item = list_first_entry(
+			&queue->queue, struct xradio_queue_item, head);
+		if (time_before(jiffies, item->queue_timestamp+queue->ttl))
+			break;
+
+		txpriv = &item->txpriv;
+		if_id = txpriv->if_id;
+		--queue->num_queued;
+		--queue->num_queued_vif[if_id];
+		--queue->link_map_cache[if_id][txpriv->link_id];
+		spin_lock_bh(&stats->lock);
+		--stats->num_queued[if_id];
+		if (!--stats->link_map_cache[if_id][txpriv->link_id])
+			wakeup_stats = true;
+		spin_unlock_bh(&stats->lock);
+		//priv = xrwl_hwpriv_to_vifpriv(stats->hw_priv, if_id);
+		//if (priv) {
+		//	xradio_debug_tx_ttl(priv);
+		//	spin_unlock(&priv->vif_lock);
+		//}
+		xradio_queue_register_post_gc(head, item);
+		item->skb = NULL;
+		list_move_tail(&item->head, &queue->free_pool);
+	}
+
+	if (wakeup_stats)
+		wake_up(&stats->wait_link_id_empty);
+	
+	//modified by yangfh for test WFD
+	if (queue->overfull) {
+		if (queue->num_queued <= ((stats->hw_priv->vif0_throttle +
+						stats->hw_priv->vif1_throttle+2)>>1)) {
+			queue->overfull = false;
+			if (unlock) {
+				__xradio_queue_unlock(queue);
+			}
+		} else if (item) {
+			unsigned long tmo = item->queue_timestamp + queue->ttl;
+			mod_timer(&queue->gc, tmo);
+			xradio_pm_stay_awake(&stats->hw_priv->pm_state,
+					tmo - jiffies);
+		}
+	}
+}
+
+static void xradio_queue_gc(struct timer_list *t)
+{
+	struct xradio_queue *queue = from_timer(queue, t, gc);
+
+	LIST_HEAD(list);
+
+	spin_lock_bh(&queue->lock);
+	__xradio_queue_gc(queue, &list, true);
+	spin_unlock_bh(&queue->lock);
+	xradio_queue_post_gc(queue->stats, &list);
+}
+
+int xradio_queue_stats_init(struct xradio_queue_stats *stats,
+			    size_t map_capacity,
+			    xradio_queue_skb_dtor_t skb_dtor,
+			    struct xradio_common *hw_priv)
+{
+	int i;
+
+	memset(stats, 0, sizeof(*stats));
+	stats->map_capacity = map_capacity;
+	stats->skb_dtor = skb_dtor;
+	stats->hw_priv = hw_priv;
+	spin_lock_init(&stats->lock);
+	init_waitqueue_head(&stats->wait_link_id_empty);
+	for (i = 0; i < XRWL_MAX_VIFS; i++) {
+		stats->link_map_cache[i] = kzalloc(sizeof(int[map_capacity]), GFP_KERNEL);
+		if (!stats->link_map_cache[i]) {
+			for (; i >= 0; i--)
+				kfree(stats->link_map_cache[i]);
+			return -ENOMEM;
+		}
+	}
+
+	return 0;
+}
+
+int xradio_queue_init(struct xradio_queue *queue,
+		      struct xradio_queue_stats *stats,
+		      u8 queue_id,
+		      size_t capacity,
+		      unsigned long ttl)
+{
+	int i;
+
+	memset(queue, 0, sizeof(*queue));
+	queue->stats = stats;
+	queue->capacity = capacity;
+	queue->queue_id = queue_id;
+	queue->ttl = ttl;
+	INIT_LIST_HEAD(&queue->queue);
+	INIT_LIST_HEAD(&queue->pending);
+	INIT_LIST_HEAD(&queue->free_pool);
+	spin_lock_init(&queue->lock);
+	timer_setup(&queue->gc, xradio_queue_gc, 0);
+
+	queue->pool = kzalloc(sizeof(struct xradio_queue_item) * capacity,
+	                         GFP_KERNEL);
+	if (!queue->pool)
+		return -ENOMEM;
+
+	for (i = 0; i < XRWL_MAX_VIFS; i++) {
+		queue->link_map_cache[i] =
+				kzalloc(sizeof(int[stats->map_capacity]), GFP_KERNEL);
+		if (!queue->link_map_cache[i]) {
+			for (; i >= 0; i--)
+				kfree(queue->link_map_cache[i]);
+			kfree(queue->pool);
+			queue->pool = NULL;
+			return -ENOMEM;
+		}
+	}
+
+	for (i = 0; i < capacity; ++i)
+		list_add_tail(&queue->pool[i].head, &queue->free_pool);
+
+	return 0;
+}
+
+/* TODO:COMBO: Flush only a particular interface specific parts */
+int xradio_queue_clear(struct xradio_queue *queue, int if_id)
+{
+	int i, cnt, iter;
+	struct xradio_queue_stats *stats = queue->stats;
+	LIST_HEAD(gc_list);
+
+	cnt = 0;
+	spin_lock_bh(&queue->lock);
+	queue->generation++;
+	queue->generation &= 0xf;
+	list_splice_tail_init(&queue->queue, &queue->pending);
+	while (!list_empty(&queue->pending)) {
+		struct xradio_queue_item *item = list_first_entry(
+			&queue->pending, struct xradio_queue_item, head);
+		WARN_ON(!item->skb);
+		if (XRWL_ALL_IFS == if_id || item->txpriv.if_id == if_id) {
+			xradio_queue_register_post_gc(&gc_list, item);
+			item->skb = NULL;
+			list_move_tail(&item->head, &queue->free_pool);
+			cnt++;
+		}
+	}
+	queue->num_queued -= cnt;
+	queue->num_pending = 0;
+	if (XRWL_ALL_IFS != if_id) {
+		queue->num_queued_vif[if_id] = 0;
+		queue->num_pending_vif[if_id] = 0;
+	} else {
+		for (iter = 0; iter < XRWL_MAX_VIFS; iter++) {
+			queue->num_queued_vif[iter] = 0;
+			queue->num_pending_vif[iter] = 0;
+		}
+	}
+	spin_lock_bh(&stats->lock);
+	if (XRWL_ALL_IFS != if_id) {
+		for (i = 0; i < stats->map_capacity; ++i) {
+			stats->num_queued[if_id] -=
+				queue->link_map_cache[if_id][i];
+			stats->link_map_cache[if_id][i] -=
+				queue->link_map_cache[if_id][i];
+			queue->link_map_cache[if_id][i] = 0;
+		}
+	} else {
+		for (iter = 0; iter < XRWL_MAX_VIFS; iter++) {
+			for (i = 0; i < stats->map_capacity; ++i) {
+				stats->num_queued[iter] -=
+					queue->link_map_cache[iter][i];
+				stats->link_map_cache[iter][i] -=
+					queue->link_map_cache[iter][i];
+				queue->link_map_cache[iter][i] = 0;
+			}
+		}
+	}
+	spin_unlock_bh(&stats->lock);
+	if (unlikely(queue->overfull)) {
+		queue->overfull = false;
+		__xradio_queue_unlock(queue);
+	}
+	spin_unlock_bh(&queue->lock);
+	wake_up(&stats->wait_link_id_empty);
+	xradio_queue_post_gc(stats, &gc_list);
+	return 0;
+}
+
+void xradio_queue_stats_deinit(struct xradio_queue_stats *stats)
+{
+	int i;
+
+	for (i = 0; i < XRWL_MAX_VIFS ; i++) {
+		kfree(stats->link_map_cache[i]);
+		stats->link_map_cache[i] = NULL;
+	}
+}
+
+void xradio_queue_deinit(struct xradio_queue *queue)
+{
+	int i;
+
+	xradio_queue_clear(queue, XRWL_ALL_IFS);
+	del_timer_sync(&queue->gc);
+	INIT_LIST_HEAD(&queue->free_pool);
+	kfree(queue->pool);
+	for (i = 0; i < XRWL_MAX_VIFS; i++) {
+		kfree(queue->link_map_cache[i]);
+		queue->link_map_cache[i] = NULL;
+	}
+	queue->pool = NULL;
+	queue->capacity = 0;
+}
+
+size_t xradio_queue_get_num_queued(struct xradio_vif *priv,
+				   struct xradio_queue *queue,
+				   u32 link_id_map)
+{
+	size_t ret;
+	int i, bit;
+	size_t map_capacity = queue->stats->map_capacity;
+
+	if (!link_id_map)
+		return 0;
+
+	spin_lock_bh(&queue->lock);
+	if (likely(link_id_map == (u32) -1)) {
+		ret = queue->num_queued_vif[priv->if_id] -
+			queue->num_pending_vif[priv->if_id];
+	} else {
+		ret = 0;
+		for (i = 0, bit = 1; i < map_capacity; ++i, bit <<= 1) {
+			if (link_id_map & bit)
+				ret +=
+				queue->link_map_cache[priv->if_id][i];
+		}
+	}
+	spin_unlock_bh(&queue->lock);
+	return ret;
+}
+
+int xradio_queue_put(struct xradio_queue *queue, struct sk_buff *skb,
+                     struct xradio_txpriv *txpriv)
+{
+	int ret = 0;
+	LIST_HEAD(gc_list);
+	struct xradio_queue_stats *stats = queue->stats;
+	/* TODO:COMBO: Add interface ID info to queue item */
+
+	if (txpriv->link_id >= queue->stats->map_capacity)
+		return -EINVAL;
+
+	spin_lock_bh(&queue->lock);
+	if (!WARN_ON(list_empty(&queue->free_pool))) {
+		struct xradio_queue_item *item = list_first_entry(
+			&queue->free_pool, struct xradio_queue_item, head);
+		BUG_ON(item->skb);
+
+		list_move_tail(&item->head, &queue->queue);
+		item->skb = skb;
+		item->txpriv = *txpriv;
+		item->generation  = 1; /* avoid packet ID is 0.*/
+		item->pack_stk_wr = 0;
+		item->packetID = xradio_queue_make_packet_id(
+			queue->generation, queue->queue_id,
+			item->generation, item - queue->pool,
+			txpriv->if_id, txpriv->raw_link_id);
+		item->queue_timestamp = jiffies;
+
+#ifdef TES_P2P_0002_ROC_RESTART
+		if (TES_P2P_0002_state == TES_P2P_0002_STATE_SEND_RESP) {
+			TES_P2P_0002_packet_id = item->packetID;
+			TES_P2P_0002_state = TES_P2P_0002_STATE_GET_PKTID;
+			txrx_printk(XRADIO_DBG_WARN, "[ROC_RESTART_STATE_GET_PKTID]\n");
+		}
+#endif
+
+		++queue->num_queued;
+		++queue->num_queued_vif[txpriv->if_id];
+		++queue->link_map_cache[txpriv->if_id][txpriv->link_id];
+
+		spin_lock_bh(&stats->lock);
+		++stats->num_queued[txpriv->if_id];
+		++stats->link_map_cache[txpriv->if_id][txpriv->link_id];
+		spin_unlock_bh(&stats->lock);
+
+		/*
+		 * TX may happen in parallel sometimes.
+		 * Leave extra queue slots so we don't overflow.
+		 */
+		if (queue->overfull == false &&
+				queue->num_queued >=
+		((stats->hw_priv->vif0_throttle +stats->hw_priv->vif1_throttle)
+				- (num_present_cpus() - 1))) {
+			queue->overfull = true;
+			__xradio_queue_lock(queue);
+			mod_timer(&queue->gc, jiffies);
+			txrx_printk(XRADIO_DBG_NIY,"!lock queue\n");
+		}
+	} else {
+		ret = -ENOENT;
+	}
+#if 0
+	txrx_printk(XRADIO_DBG_ERROR, "queue_put queue %d, %d, %d\n",
+		queue->num_queued,
+		queue->link_map_cache[txpriv->if_id][txpriv->link_id],
+		queue->num_pending);
+	txrx_printk(XRADIO_DBG_ERROR, "queue_put stats %d, %d\n", stats->num_queued,
+		stats->link_map_cache[txpriv->if_id][txpriv->link_id]);
+#endif
+	spin_unlock_bh(&queue->lock);
+	return ret;
+}
+
+int xradio_queue_get(struct xradio_queue *queue,
+			int if_id,
+		     u32 link_id_map,
+		     struct wsm_tx **tx,
+		     struct ieee80211_tx_info **tx_info,
+		     struct xradio_txpriv **txpriv)
+{
+	int ret = -ENOENT;
+	struct xradio_queue_item *item;
+	struct xradio_queue_stats *stats = queue->stats;
+	bool wakeup_stats = false;
+
+	spin_lock_bh(&queue->lock);
+	list_for_each_entry(item, &queue->queue, head) {
+		if ((item->txpriv.if_id == if_id) &&
+			(link_id_map & BIT(item->txpriv.link_id))) {
+			ret = 0;
+			break;
+		}
+	}
+
+	if (!WARN_ON(ret)) {
+		*tx = (struct wsm_tx *)item->skb->data;
+		*tx_info = IEEE80211_SKB_CB(item->skb);
+		*txpriv = &item->txpriv;
+		(*tx)->packetID = __cpu_to_le32(item->packetID);
+		list_move_tail(&item->head, &queue->pending);
+		++queue->num_pending;
+		++queue->num_pending_vif[item->txpriv.if_id];
+		--queue->link_map_cache[item->txpriv.if_id]
+				[item->txpriv.link_id];
+		item->xmit_timestamp = jiffies;
+
+		spin_lock_bh(&stats->lock);
+		--stats->num_queued[item->txpriv.if_id];
+		if (!--stats->link_map_cache[item->txpriv.if_id]
+					[item->txpriv.link_id])
+			wakeup_stats = true;
+
+		spin_unlock_bh(&stats->lock);
+#if 0
+		txrx_printk(XRADIO_DBG_ERROR, "queue_get queue %d, %d, %d\n",
+		queue->num_queued,
+		queue->link_map_cache[item->txpriv.if_id][item->txpriv.link_id],
+		queue->num_pending);
+		txrx_printk(XRADIO_DBG_ERROR, "queue_get stats %d, %d\n", stats->num_queued,
+		stats->link_map_cache[item->txpriv.if_id]
+		[item->txpriv.link_id]);
+#endif
+	}
+	spin_unlock_bh(&queue->lock);
+	if (wakeup_stats)
+		wake_up(&stats->wait_link_id_empty);
+
+	return ret;
+}
+
+int xradio_queue_requeue(struct xradio_queue *queue, u32 packetID, bool check)
+{
+	int ret = 0;
+	u8 queue_generation, queue_id, item_generation, item_id, if_id, link_id;
+	struct xradio_queue_item *item;
+	struct xradio_queue_stats *stats = queue->stats;
+
+	xradio_queue_parse_id(packetID, &queue_generation, &queue_id,
+				&item_generation, &item_id, &if_id, &link_id);
+
+	item = &queue->pool[item_id];
+	if (check && item->txpriv.offchannel_if_id == XRWL_GENERIC_IF_ID) {
+		txrx_printk(XRADIO_DBG_MSG, "Requeued frame dropped for "
+						"generic interface id.\n");
+		xradio_queue_remove(queue, packetID);
+		return 0;
+	}
+
+	if (!check)
+		item->txpriv.offchannel_if_id = XRWL_GENERIC_IF_ID;
+
+	/*if_id = item->txpriv.if_id;*/
+
+	spin_lock_bh(&queue->lock);
+	BUG_ON(queue_id != queue->queue_id);
+	if (unlikely(queue_generation != queue->generation)) {
+		ret = -ENOENT;
+	} else if (unlikely(item_id >= (unsigned) queue->capacity)) {
+		WARN_ON(1);
+		ret = -EINVAL;
+	} else if (unlikely(item->generation != item_generation)) {
+		WARN_ON(1);
+		ret = -ENOENT;
+	} else {
+		--queue->num_pending;
+		--queue->num_pending_vif[if_id];
+		++queue->link_map_cache[if_id][item->txpriv.link_id];
+
+		spin_lock_bh(&stats->lock);
+		++stats->num_queued[item->txpriv.if_id];
+		++stats->link_map_cache[if_id][item->txpriv.link_id];
+		spin_unlock_bh(&stats->lock);
+
+		item->generation = ++item_generation;
+		item->packetID = xradio_queue_make_packet_id(
+			queue_generation, queue_id, item_generation, item_id,
+			if_id, link_id);
+		list_move(&item->head, &queue->queue);
+#if 0
+		txrx_printk(XRADIO_DBG_ERROR, "queue_requeue queue %d, %d, %d\n",
+		queue->num_queued,
+		queue->link_map_cache[if_id][item->txpriv.link_id],
+		queue->num_pending);
+		txrx_printk(XRADIO_DBG_ERROR, "queue_requeue stats %d, %d\n",
+		stats->num_queued,
+		stats->link_map_cache[if_id][item->txpriv.link_id]);
+#endif
+	}
+	spin_unlock_bh(&queue->lock);
+	return ret;
+}
+
+int xradio_queue_requeue_all(struct xradio_queue *queue)
+{
+	struct xradio_queue_stats *stats = queue->stats;
+	spin_lock_bh(&queue->lock);
+	while (!list_empty(&queue->pending)) {
+		struct xradio_queue_item *item = list_entry(
+			queue->pending.prev, struct xradio_queue_item, head);
+
+		--queue->num_pending;
+		--queue->num_pending_vif[item->txpriv.if_id];
+		++queue->link_map_cache[item->txpriv.if_id]
+				[item->txpriv.link_id];
+
+		spin_lock_bh(&stats->lock);
+		++stats->num_queued[item->txpriv.if_id];
+		++stats->link_map_cache[item->txpriv.if_id]
+				[item->txpriv.link_id];
+		spin_unlock_bh(&stats->lock);
+
+		++item->generation;
+		item->packetID = xradio_queue_make_packet_id(
+			queue->generation, queue->queue_id,
+			item->generation, item - queue->pool,
+			item->txpriv.if_id, item->txpriv.raw_link_id);
+		list_move(&item->head, &queue->queue);
+	}
+	spin_unlock_bh(&queue->lock);
+
+	return 0;
+}
+
+int xradio_queue_remove(struct xradio_queue *queue, u32 packetID)
+{
+	int ret = 0;
+	u8 queue_generation, queue_id, item_generation, item_id, if_id, link_id;
+	struct xradio_queue_item *item;
+	struct xradio_queue_stats *stats = queue->stats;
+	struct sk_buff *gc_skb = NULL;
+	struct xradio_txpriv gc_txpriv;
+
+	xradio_queue_parse_id(packetID, &queue_generation, &queue_id,
+				&item_generation, &item_id, &if_id, &link_id);
+
+	item = &queue->pool[item_id];
+
+	spin_lock_bh(&queue->lock);
+	BUG_ON(queue_id != queue->queue_id);
+	/*TODO:COMBO:Add check for interface ID also */
+	if (unlikely(queue_generation != queue->generation)) {
+		ret = -ENOENT;
+	} else if (unlikely(item_id >= (unsigned) queue->capacity)) {
+		WARN_ON(1);
+		ret = -EINVAL;
+	} else if (unlikely(item->generation != item_generation)) {
+		WARN_ON(1);
+		ret = -ENOENT;
+	} else {
+		gc_txpriv = item->txpriv;
+		gc_skb = item->skb;
+		item->skb = NULL;
+		--queue->num_pending;
+		--queue->num_pending_vif[if_id];
+		--queue->num_queued;
+		--queue->num_queued_vif[if_id];
+		++queue->num_sent;
+		++item->generation;
+
+		/* Do not use list_move_tail here, but list_move:
+		 * try to utilize cache row.
+		 */
+		list_move(&item->head, &queue->free_pool);
+
+		if (unlikely(queue->overfull) &&
+		    (queue->num_queued <= ((stats->hw_priv->vif0_throttle + stats->hw_priv->vif1_throttle + 2)>>1))) {
+			queue->overfull = false;
+			__xradio_queue_unlock(queue);
+		}
+	}
+	spin_unlock_bh(&queue->lock);
+
+#if 0
+	txrx_printk(XRADIO_DBG_ERROR, "queue_drop queue %d, %d, %d\n",
+		queue->num_queued, queue->link_map_cache[if_id][0],
+		queue->num_pending);
+	txrx_printk(XRADIO_DBG_ERROR, "queue_drop stats %d, %d\n", stats->num_queued,
+		stats->link_map_cache[if_id][0]);
+#endif
+	if (gc_skb)
+		stats->skb_dtor(stats->hw_priv, gc_skb, &gc_txpriv);
+
+	return ret;
+}
+
+int xradio_queue_get_skb(struct xradio_queue *queue, u32 packetID,
+			 struct sk_buff **skb,
+			 const struct xradio_txpriv **txpriv)
+{
+	int ret = 0;
+	u8 queue_generation, queue_id, item_generation, item_id, if_id, link_id;
+	struct xradio_queue_item *item;
+
+	xradio_queue_parse_id(packetID, &queue_generation, &queue_id,
+				&item_generation, &item_id, &if_id, &link_id);
+
+	item = &queue->pool[item_id];
+
+	spin_lock_bh(&queue->lock);
+	BUG_ON(queue_id != queue->queue_id);
+	/* TODO:COMBO: Add check for interface ID here */
+	if (unlikely(queue_generation != queue->generation)) {
+		ret = -ENOENT;
+	} else if (unlikely(item_id >= (unsigned) queue->capacity)) {
+		WARN_ON(1);
+		ret = -EINVAL;
+	} else if (unlikely(item->generation != item_generation)) {
+		WARN_ON(1);
+		ret = -ENOENT;
+	} else {
+		*skb = item->skb;
+		*txpriv = &item->txpriv;
+	}
+	spin_unlock_bh(&queue->lock);
+	return ret;
+}
+
+void xradio_queue_lock(struct xradio_queue *queue)
+{
+	spin_lock_bh(&queue->lock);
+	__xradio_queue_lock(queue);
+	spin_unlock_bh(&queue->lock);
+}
+
+void xradio_queue_unlock(struct xradio_queue *queue)
+{
+	spin_lock_bh(&queue->lock);
+	__xradio_queue_unlock(queue);
+	spin_unlock_bh(&queue->lock);
+}
+
+bool xradio_queue_get_xmit_timestamp(struct xradio_queue *queue,
+				     unsigned long *timestamp, int if_id,
+				     u32 pending_frameID, u32 *Old_frame_ID)
+{
+	struct xradio_queue_item *item;
+	bool ret;
+
+	spin_lock_bh(&queue->lock);
+	ret = !list_empty(&queue->pending);
+	if (ret) {
+		list_for_each_entry(item, &queue->pending, head) {
+			if (((if_id == XRWL_GENERIC_IF_ID) ||
+				(if_id == XRWL_ALL_IFS) ||
+					(item->txpriv.if_id == if_id)) &&
+					(item->packetID != pending_frameID)) {
+				if (time_before(item->xmit_timestamp,
+							*timestamp)) {
+					*timestamp = item->xmit_timestamp;
+					*Old_frame_ID = item->packetID;
+				}
+			}
+		}
+	}
+	spin_unlock_bh(&queue->lock);
+	return ret;
+}
+
+bool xradio_queue_stats_is_empty(struct xradio_queue_stats *stats,
+				 u32 link_id_map, int if_id)
+{
+	bool empty = true;
+
+	spin_lock_bh(&stats->lock);
+	if (link_id_map == (u32)-1)
+		empty = stats->num_queued[if_id] == 0;
+	else {
+		int i, if_id;
+		for (if_id = 0; if_id < XRWL_MAX_VIFS; if_id++) {
+			for (i = 0; i < stats->map_capacity; ++i) {
+				if (link_id_map & BIT(i)) {
+					if (stats->link_map_cache[if_id][i]) {
+						empty = false;
+						break;
+					}
+				}
+			}
+		}
+	}
+	spin_unlock_bh(&stats->lock);
+
+	return empty;
+}
+
+bool xradio_query_txpkt_timeout(struct xradio_common *hw_priv, int if_id,
+                                u32 pending_pkt_id, long *timeout)
+{
+	int i;
+	bool pending = false;
+	unsigned long timestamp = jiffies;
+	struct xradio_queue      *queue = NULL;
+	struct xradio_queue_item *item  = NULL;
+	struct xradio_queue      *old_queue = NULL;
+	struct xradio_queue_item *old_item  = NULL;
+	u8 pack_stk_wr = 0;
+
+	/* Get oldest frame.*/
+	for (i = 0; i < AC_QUEUE_NUM; ++i) {
+		queue = &hw_priv->tx_queue[i];
+		spin_lock_bh(&queue->lock);
+		if (!list_empty(&queue->pending)) {
+			list_for_each_entry(item, &queue->pending, head) {
+				if (((if_id == XRWL_GENERIC_IF_ID) ||
+					 (if_id == XRWL_ALL_IFS) ||
+					 (item->txpriv.if_id == if_id)) &&
+					 (item->packetID != pending_pkt_id)) {
+					if (time_before(item->xmit_timestamp, timestamp)) {
+						timestamp   = item->xmit_timestamp;
+						pack_stk_wr = item->pack_stk_wr;
+						old_queue   = queue;
+						old_item    = item;
+					}
+				}
+			}
+			pending = true;
+		}
+		spin_unlock_bh(&queue->lock);
+	}
+	if (!pending)
+		return false;
+
+	/* Check if frame transmission is timed out.
+	 * add (WSM_CMD_LAST_CHANCE_TIMEOUT>>1) for stuck workaround.*/
+	*timeout = timestamp + WSM_CMD_LAST_CHANCE_TIMEOUT - jiffies;
+	if (unlikely(*timeout < 0) && !pack_stk_wr) {
+		struct ieee80211_hdr *frame = NULL;
+		const struct xradio_txpriv *txpriv = NULL;
+		u16 fctl = 0x0;
+		u32 len  = 0x0;
+		u8 if_id = 0, link_id = 0, tid = 0;
+
+		/* query the timeout frame. */
+		spin_lock_bh(&old_queue->lock);
+		if (likely(old_item->skb && !hw_priv->query_packetID)) {
+			hw_priv->query_packetID = old_item->packetID;
+			old_item->pack_stk_wr = 1;
+			atomic_add(1, &hw_priv->query_cnt);
+
+			/* Info of stuck frames for debug.*/
+			txpriv = &old_item->txpriv;
+			frame  = (struct ieee80211_hdr *)(&old_item->skb->data[txpriv->offset]);
+			fctl   = frame->frame_control;
+			len    = old_item->skb->len;
+			if_id  = txpriv->if_id;
+			link_id = txpriv->link_id;
+			tid = txpriv->tid;
+		}
+		spin_unlock_bh(&old_queue->lock);
+		/* Dump Info of stuck frames. */
+		if (frame) {
+			txrx_printk(XRADIO_DBG_ERROR, "TX confirm timeout(%ds).\n", 
+			            WSM_CMD_LAST_CHANCE_TIMEOUT/HZ);
+			txrx_printk(XRADIO_DBG_ERROR, "if=%d, linkid=%d, tid=%d, " \
+			            "old_packetID=0x%08x, fctl=0x%04x, len=%d, wr=%d\n", 
+			            if_id, link_id, tid,  hw_priv->query_packetID, fctl, len, 
+			            pack_stk_wr);
+		}
+		/* Return half of timeout for query packet. */
+		*timeout = (WSM_CMD_LAST_CHANCE_TIMEOUT>>1);
+	} else if (unlikely(pack_stk_wr)){
+		*timeout = *timeout + (WSM_CMD_LAST_CHANCE_TIMEOUT>>1);
+		txrx_printk(XRADIO_DBG_MSG,"%s, wr and timeout=%ld\n", __func__, *timeout);
+	}
+	return pending;
+}
diff --git a/drivers/net/wireless/xradio/queue.h b/drivers/net/wireless/xradio/queue.h
new file mode 100644
index 0000000..d6b64ce
--- /dev/null
+++ b/drivers/net/wireless/xradio/queue.h
@@ -0,0 +1,139 @@
+/*
+ * queue operations for XRadio drivers
+ *
+ * Copyright (c) 2013, XRadio
+ * Author: XRadio
+ *
+ * This program is free software; you can 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 XRADIO_QUEUE_H_INCLUDED
+#define XRADIO_QUEUE_H_INCLUDED
+
+/* private */ struct xradio_queue_item;
+
+/* extern */ struct sk_buff;
+/* extern */ struct wsm_tx;
+/* extern */ struct xradio_common;
+/* extern */ struct xradio_vif;
+/* extern */ struct ieee80211_tx_queue_stats;
+/* extern */ struct xradio_txpriv;
+
+/* forward */ struct xradio_queue_stats;
+
+typedef void (*xradio_queue_skb_dtor_t)(struct xradio_common *priv,
+                                        struct sk_buff *skb,
+                                        const struct xradio_txpriv *txpriv);
+
+struct xradio_queue {
+	struct                    xradio_queue_stats *stats;
+	size_t                    capacity;
+	size_t                    num_queued;
+	size_t                    num_queued_vif[XRWL_MAX_VIFS];
+	size_t                    num_pending;
+	size_t                    num_pending_vif[XRWL_MAX_VIFS];
+	size_t                    num_sent;
+	struct xradio_queue_item *pool;
+	struct list_head          queue;
+	struct list_head          free_pool;
+	struct list_head          pending;
+	int                       tx_locked_cnt;
+	int                      *link_map_cache[XRWL_MAX_VIFS];
+	bool                      overfull;
+	spinlock_t                lock;
+	u8                        queue_id;
+	u8                        generation;
+	struct timer_list	        gc;
+	unsigned long             ttl;
+};
+
+struct xradio_queue_stats {
+	spinlock_t              lock;
+	int                    *link_map_cache[XRWL_MAX_VIFS];
+	int                     num_queued[XRWL_MAX_VIFS];
+	size_t                  map_capacity;
+	wait_queue_head_t       wait_link_id_empty;
+	xradio_queue_skb_dtor_t skb_dtor;
+	struct xradio_common   *hw_priv;
+};
+
+struct xradio_txpriv {
+	u8 link_id;
+	u8 raw_link_id;
+	u8 tid;
+	u8 rate_id;
+	u8 offset;
+	u8 if_id;
+	u8 offchannel_if_id;
+	u8 use_bg_rate;
+};
+
+int xradio_queue_stats_init(struct xradio_queue_stats *stats,
+                            size_t map_capacity,
+                            xradio_queue_skb_dtor_t skb_dtor,
+                            struct xradio_common *priv);
+int xradio_queue_init(struct xradio_queue *queue,
+                      struct xradio_queue_stats *stats,
+                      u8 queue_id,
+                      size_t capacity,
+                      unsigned long ttl);
+int xradio_queue_clear(struct xradio_queue *queue, int if_id);
+void xradio_queue_stats_deinit(struct xradio_queue_stats *stats);
+void xradio_queue_deinit(struct xradio_queue *queue);
+
+size_t xradio_queue_get_num_queued(struct xradio_vif *priv,
+                                   struct xradio_queue *queue,
+                                   u32 link_id_map);
+int xradio_queue_put(struct xradio_queue *queue,
+                     struct sk_buff *skb, struct xradio_txpriv *txpriv);
+int xradio_queue_get(struct xradio_queue *queue,
+                     int if_id, u32 link_id_map,
+                     struct wsm_tx **tx,
+                     struct ieee80211_tx_info **tx_info,
+                     struct xradio_txpriv **txpriv);
+
+int xradio_queue_requeue(struct xradio_queue *queue, u32 packetID, bool check);
+
+int xradio_queue_requeue_all(struct xradio_queue *queue);
+int xradio_queue_remove(struct xradio_queue *queue,
+                        u32 packetID);
+
+int xradio_queue_get_skb(struct xradio_queue *queue, u32 packetID,
+                         struct sk_buff **skb,
+                         const struct xradio_txpriv **txpriv);
+void xradio_queue_lock(struct xradio_queue *queue);
+void xradio_queue_unlock(struct xradio_queue *queue);
+bool xradio_queue_get_xmit_timestamp(struct xradio_queue *queue,
+                                     unsigned long *timestamp, int if_id,
+                                     u32 pending_frameID, u32 *Old_frame_ID);
+bool xradio_query_txpkt_timeout(struct xradio_common *hw_priv, int if_id,
+                                u32 pending_pkt_id, long *timeout);
+
+
+bool xradio_queue_stats_is_empty(struct xradio_queue_stats *stats,
+                                 u32 link_id_map, int if_id);
+
+static inline u8 xradio_queue_get_queue_id(u32 packetID)
+{
+	return (packetID >> 16) & 0xF;
+}
+
+static inline u8 xradio_queue_get_if_id(u32 packetID)
+{
+	return (packetID >> 20) & 0xF;
+}
+
+static inline u8 xradio_queue_get_link_id(u32 packetID)
+{
+	return (packetID >> 24) & 0xF;
+}
+
+static inline u8 xradio_queue_get_generation(u32 packetID)
+{
+	return (packetID >>  8) & 0xFF;
+}
+
+#endif /* XRADIO_QUEUE_H_INCLUDED */
diff --git a/drivers/net/wireless/xradio/rx.c b/drivers/net/wireless/xradio/rx.c
new file mode 100644
index 0000000..fb8be92
--- /dev/null
+++ b/drivers/net/wireless/xradio/rx.c
@@ -0,0 +1,414 @@
+#include <net/mac80211.h>
+
+#include "xradio.h"
+#include "rx.h"
+#include "ht.h"
+#include "p2p.h"
+#include "sta.h"
+#include "bh.h"
+#include "ap.h"
+
+	// MRK: added copy of this tx.c function here for testing, renamed _rx
+
+static void xradio_check_go_neg_conf_success_rx(struct xradio_common *hw_priv,
+						u8 *action)
+{
+	if (action[2] == 0x50 && action[3] == 0x6F && action[4] == 0x9A &&
+		action[5] == 0x09 && action[6] == 0x02) {
+		if(action[17] == 0) {
+			hw_priv->is_go_thru_go_neg = true;
+		}
+		else {
+			hw_priv->is_go_thru_go_neg = false;
+		}
+	}
+}
+
+
+static int xradio_handle_pspoll(struct xradio_vif *priv,
+				struct sk_buff *skb)
+{
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	struct ieee80211_sta *sta;
+	struct ieee80211_pspoll *pspoll =
+		(struct ieee80211_pspoll *) skb->data;
+	int link_id = 0;
+	u32 pspoll_mask = 0;
+	int drop = 1;
+	int i;
+
+
+	if (priv->join_status != XRADIO_JOIN_STATUS_AP)
+		goto done;
+	if (memcmp(priv->vif->addr, pspoll->bssid, ETH_ALEN))
+		goto done;
+
+	rcu_read_lock();
+	sta = ieee80211_find_sta(priv->vif, pspoll->ta);
+	if (sta) {
+		struct xradio_sta_priv *sta_priv;
+		sta_priv = (struct xradio_sta_priv *)&sta->drv_priv;
+		link_id = sta_priv->link_id;
+		pspoll_mask = BIT(sta_priv->link_id);
+	}
+	rcu_read_unlock();
+	if (!link_id)
+		goto done;
+
+	priv->pspoll_mask |= pspoll_mask;
+	drop = 0;
+
+	/* Do not report pspols if data for given link id is
+	 * queued already. */
+	for (i = 0; i < 4; ++i) {
+		if (xradio_queue_get_num_queued(priv,
+				&hw_priv->tx_queue[i],
+				pspoll_mask)) {
+			xradio_bh_wakeup(hw_priv);
+			drop = 1;
+			break;
+		}
+	}
+	txrx_printk(XRADIO_DBG_NIY, "[RX] PSPOLL: %s\n", drop ? "local" : "fwd");
+done:
+	return drop;
+}
+
+
+static void
+xradio_rx_h_ba_stat(struct xradio_vif *priv,
+		    size_t hdrlen, size_t skb_len )
+{
+	struct xradio_common *hw_priv = priv->hw_priv;
+
+
+	if (priv->join_status != XRADIO_JOIN_STATUS_STA)
+		return;
+	if (!xradio_is_ht(&hw_priv->ht_oper))
+		return;
+	if (!priv->setbssparams_done)
+		return;
+
+	spin_lock_bh(&hw_priv->ba_lock);
+	hw_priv->ba_acc_rx += skb_len - hdrlen;
+	if (!(hw_priv->ba_cnt_rx || hw_priv->ba_cnt)) {
+		mod_timer(&hw_priv->ba_timer,
+			jiffies + XRADIO_BLOCK_ACK_INTERVAL);
+	}
+	hw_priv->ba_cnt_rx++;
+	spin_unlock_bh(&hw_priv->ba_lock);
+}
+
+void xradio_rx_cb(struct xradio_vif *priv,
+		  struct wsm_rx *arg,
+		  struct sk_buff **skb_p)
+{
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	struct sk_buff *skb = *skb_p;
+	struct ieee80211_rx_status *hdr = IEEE80211_SKB_RXCB(skb);
+	struct ieee80211_hdr *frame = (struct ieee80211_hdr *)skb->data;
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
+#endif
+	struct xradio_link_entry *entry = NULL;
+	unsigned long grace_period;
+	bool early_data = false;
+	size_t hdrlen = 0;
+	u8   parse_iv_len = 0;
+
+	dev_dbg(hw_priv->pdev, "vif %d: rx, status %u flags 0x%.8x",
+			priv->if_id, arg->status, arg->flags);
+	if(ieee80211_is_deauth(frame->frame_control))
+		dev_dbg(hw_priv->pdev, "vif %d: deauth\n", priv->if_id);
+
+	hdr->flag = 0;
+
+	if (unlikely(priv->mode == NL80211_IFTYPE_UNSPECIFIED)) {
+		/* STA is stopped. */
+		goto drop;
+	}
+	
+#ifdef TES_P2P_0002_ROC_RESTART
+	xradio_frame_monitor(hw_priv,skb,false);
+#endif
+
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+	if ((ieee80211_is_action(frame->frame_control))
+	    && (mgmt->u.action.category == WLAN_CATEGORY_PUBLIC)) {
+		u8 *action = (u8*)&mgmt->u.action.category;
+		xradio_check_go_neg_conf_success_rx(hw_priv, action);
+	}
+#endif
+
+	if (arg->link_id && (arg->link_id != XRADIO_LINK_ID_UNMAPPED)
+			&& (arg->link_id <= XRADIO_MAX_STA_IN_AP_MODE)) {
+		entry =	&priv->link_id_db[arg->link_id - 1];
+		if (entry->status == XRADIO_LINK_SOFT &&
+				ieee80211_is_data(frame->frame_control))
+			early_data = true;
+		entry->timestamp = jiffies;
+	}
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+	else if ((arg->link_id == XRADIO_LINK_ID_UNMAPPED)
+			&& (priv->vif->p2p == WSM_START_MODE_P2P_GO)
+			&& ieee80211_is_action(frame->frame_control)
+			&& (mgmt->u.action.category == WLAN_CATEGORY_PUBLIC)) {
+		txrx_printk(XRADIO_DBG_NIY, "[RX] Going to MAP&RESET link ID\n");
+
+		if (work_pending(&priv->linkid_reset_work))
+			WARN_ON(1);
+
+		memcpy(&priv->action_frame_sa[0],
+				ieee80211_get_SA(frame), ETH_ALEN);
+		priv->action_linkid = 0;
+		schedule_work(&priv->linkid_reset_work);
+	}
+
+	if (arg->link_id && (arg->link_id != XRADIO_LINK_ID_UNMAPPED)
+			&& (priv->vif->p2p == WSM_START_MODE_P2P_GO)
+			&& ieee80211_is_action(frame->frame_control)
+			&& (mgmt->u.action.category == WLAN_CATEGORY_PUBLIC)) {
+		/* Link ID already exists for the ACTION frame.
+		 * Reset and Remap */
+		if (work_pending(&priv->linkid_reset_work))
+			WARN_ON(1);
+		memcpy(&priv->action_frame_sa[0],
+				ieee80211_get_SA(frame), ETH_ALEN);
+		priv->action_linkid = arg->link_id;
+		schedule_work(&priv->linkid_reset_work);
+	}
+#endif
+	if (unlikely(arg->status)) {
+		if (arg->status == WSM_STATUS_MICFAILURE) {
+			dev_err(priv->hw_priv->pdev, "[RX] IF=%d, MIC failure.\n",
+			            priv->if_id);
+			hdr->flag |= RX_FLAG_MMIC_ERROR;
+		} else if (arg->status == WSM_STATUS_NO_KEY_FOUND) {
+			dev_warn(priv->hw_priv->pdev, "received frame has no key status\n");
+			//goto drop;
+		} else {
+			dev_err(priv->hw_priv->pdev, "[RX] IF=%d, Receive failure: %d.\n",
+				priv->if_id, arg->status);
+			goto drop;
+		}
+	}
+
+	if (skb->len < sizeof(struct ieee80211_pspoll)) {
+		dev_err(priv->hw_priv->pdev, "Malformed SDU rx'ed. "
+		            "Size is lesser than IEEE header.\n");
+		goto drop;
+	}
+
+	if (unlikely(ieee80211_is_pspoll(frame->frame_control)))
+		if (xradio_handle_pspoll(priv, skb))
+			goto drop;
+
+	hdr->mactime = 0; /* Not supported by WSM */
+	hdr->band = (arg->channelNumber > 14) ?
+			NL80211_BAND_5GHZ : NL80211_BAND_2GHZ;
+	hdr->freq = ieee80211_channel_to_frequency(
+			arg->channelNumber,
+			hdr->band);
+
+#ifdef AP_HT_COMPAT_FIX
+	if (!priv->ht_compat_det && priv->htcap &&
+		ieee80211_is_data_qos(frame->frame_control)) {
+		if(xradio_apcompat_detect(priv, arg->rxedRate))
+			goto drop;
+	}
+#endif
+       
+	if (arg->rxedRate >= 14) {
+		hdr->encoding = RX_ENC_HT;
+		hdr->rate_idx = arg->rxedRate - 14;
+	} else if (arg->rxedRate >= 4) {
+		if (hdr->band == NL80211_BAND_5GHZ)
+			hdr->rate_idx = arg->rxedRate - 6;
+		else
+			hdr->rate_idx = arg->rxedRate - 2;
+	} else {
+		hdr->rate_idx = arg->rxedRate;
+	}
+
+	hdr->signal = (s8)arg->rcpiRssi;
+	hdr->antenna = 0;
+
+	hdrlen = ieee80211_hdrlen(frame->frame_control);
+
+	if (WSM_RX_STATUS_ENCRYPTION(arg->flags)) {
+		size_t iv_len = 0, icv_len = 0;
+
+		hdr->flag |= RX_FLAG_DECRYPTED;
+
+		/* Oops... There is no fast way to ask mac80211 about
+		 * IV/ICV lengths. Even defines are not exposed.*/
+		switch (WSM_RX_STATUS_ENCRYPTION(arg->flags)) {
+		case WSM_RX_STATUS_WEP:
+			iv_len = 4 /* WEP_IV_LEN */;
+			icv_len = 4 /* WEP_ICV_LEN */;
+			break;
+		case WSM_RX_STATUS_TKIP:
+			iv_len = 8 /* TKIP_IV_LEN */;
+			icv_len = 4 /* TKIP_ICV_LEN */
+				+ 8 /*MICHAEL_MIC_LEN*/;
+			break;
+		case WSM_RX_STATUS_AES:
+			iv_len = 8 /* CCMP_HDR_LEN */;
+			icv_len = 8 /* CCMP_MIC_LEN */;
+			break;
+		case WSM_RX_STATUS_WAPI:
+			iv_len = 18 /* WAPI_HDR_LEN */;
+			icv_len = 16 /* WAPI_MIC_LEN */;
+			hdr->flag |= RX_FLAG_IV_STRIPPED;
+			break;
+		default:
+			WARN_ON("Unknown encryption type");
+			goto drop;
+		}
+
+		/* Firmware strips ICV in case of MIC failure. */
+		if (arg->status == WSM_STATUS_MICFAILURE) {
+			icv_len = 0;
+			hdr->flag |= RX_FLAG_IV_STRIPPED;
+		}
+
+		if (skb->len < hdrlen + iv_len + icv_len) {
+			dev_err(priv->hw_priv->pdev, "Mailformed SDU rx'ed. "
+				"Size is lesser than crypto headers.\n");
+			goto drop;
+		}
+
+		if (WSM_RX_STATUS_ENCRYPTION(arg->flags) ==
+		    WSM_RX_STATUS_TKIP) {
+			/* Remove TKIP MIC 8 bytes*/
+			memmove(skb->data + skb->len-icv_len, 
+			        skb->data + skb->len-icv_len+8, 4);
+			skb_trim(skb, skb->len - 8);
+			hdr->flag |= RX_FLAG_MMIC_STRIPPED;
+		} else if (unlikely(WSM_RX_STATUS_ENCRYPTION(arg->flags) ==
+		           WSM_RX_STATUS_WAPI)) {
+			/* Protocols not defined in mac80211 should be
+			   stripped/crypted in driver/firmware */
+			/* Remove IV, ICV and MIC */
+			skb_trim(skb, skb->len - icv_len);
+			memmove(skb->data + iv_len, skb->data, hdrlen);
+			skb_pull(skb, iv_len);
+		}
+		parse_iv_len = iv_len;
+	}
+
+	if (ieee80211_is_beacon(frame->frame_control) &&
+		!arg->status &&
+		!memcmp(ieee80211_get_SA(frame), priv->join_bssid,ETH_ALEN)) {
+		const u8 *tim_ie;
+		u8 *ies;
+		size_t ies_len;
+		priv->disable_beacon_filter = false;
+		queue_work(hw_priv->workqueue, &priv->update_filtering_work);
+		ies = ((struct ieee80211_mgmt *)
+			  (skb->data))->u.beacon.variable;
+		ies_len = skb->len - (ies - (u8 *)(skb->data));
+
+		tim_ie = xradio_get_ie(ies, ies_len, WLAN_EID_TIM);
+		if (tim_ie) {
+			struct ieee80211_tim_ie *tim =
+				(struct ieee80211_tim_ie *)&tim_ie[2];
+
+			if (priv->join_dtim_period != tim->dtim_period) {
+				priv->join_dtim_period = tim->dtim_period;
+				queue_work(hw_priv->workqueue,
+					&priv->set_beacon_wakeup_period_work);
+			}
+		}
+		if (unlikely(priv->disable_beacon_filter)) {
+			priv->disable_beacon_filter = false;
+			queue_work(hw_priv->workqueue,
+				&priv->update_filtering_work);
+		}
+	}
+#ifdef AP_HT_CAP_UPDATE
+    if (priv->mode == NL80211_IFTYPE_AP           &&
+        ieee80211_is_beacon(frame->frame_control) &&
+        ((priv->ht_oper&HT_INFO_MASK) != 0x0011)  &&
+        !arg->status){
+        u8 *ies;
+        size_t ies_len;
+        const u8 *ht_cap;
+        ies = ((struct ieee80211_mgmt *)(skb->data))->u.beacon.variable;
+        ies_len = skb->len - (ies - (u8 *)(skb->data));
+        ht_cap = xradio_get_ie(ies, ies_len, WLAN_EID_HT_CAPABILITY);
+        if(!ht_cap) {
+            priv->ht_oper |= 0x0011;
+            queue_work(hw_priv->workqueue, &priv->ht_oper_update_work);
+        }
+    }
+#endif
+
+#ifdef AP_HT_COMPAT_FIX
+	if (ieee80211_is_mgmt(frame->frame_control) && 
+		priv->if_id == 0 && !(priv->ht_compat_det & 0x10)) {
+		xradio_remove_ht_ie(priv, skb);
+	}
+#endif
+
+#ifdef ROAM_OFFLOAD
+	if ((ieee80211_is_beacon(frame->frame_control)||ieee80211_is_probe_resp(frame->frame_control)) &&
+			!arg->status ) {
+		if (hw_priv->auto_scanning && !atomic_read(&hw_priv->scan.in_progress))
+			hw_priv->frame_rcvd = 1;
+
+		if (!memcmp(ieee80211_get_SA(frame), priv->join_bssid, ETH_ALEN)) {
+			if (hw_priv->beacon)
+				dev_kfree_skb(hw_priv->beacon);
+			hw_priv->beacon = skb_copy(skb, GFP_ATOMIC);
+			if (!hw_priv->beacon)
+				txrx_printk(XRADIO_DBG_ERROR, "sched_scan: own beacon storing failed\n");
+		}
+	}
+#endif /*ROAM_OFFLOAD*/
+
+	//don't delay scan before next connect, yangfh.
+	if (ieee80211_is_deauth(frame->frame_control) ||
+	    ieee80211_is_disassoc(frame->frame_control))
+		hw_priv->connet_time[priv->if_id] = 0;
+
+	/* Stay awake for 1sec. after frame is received to give
+	 * userspace chance to react and acquire appropriate
+	 * wakelock. */
+	if (ieee80211_is_auth(frame->frame_control))
+		grace_period = 5 * HZ;
+	else if (ieee80211_is_deauth(frame->frame_control))
+		grace_period = 5 * HZ;
+	else
+		grace_period = HZ;
+
+	if (ieee80211_is_data(frame->frame_control))
+		xradio_rx_h_ba_stat(priv, hdrlen, skb->len);
+
+	xradio_pm_stay_awake(&hw_priv->pm_state, grace_period);
+
+	if(xradio_realloc_resv_skb(hw_priv, *skb_p)) {
+		*skb_p = NULL;
+		return;
+	}
+	/* Try to  a packet for the case dev_alloc_skb failed in bh.*/
+	if (unlikely(early_data)) {
+		spin_lock_bh(&priv->ps_state_lock);
+		/* Double-check status with lock held */
+		if (entry->status == XRADIO_LINK_SOFT) {
+			skb_queue_tail(&entry->rx_queue, skb);
+			dev_warn(priv->hw_priv->pdev, "***skb_queue_tail\n");
+		} else
+			ieee80211_rx_irqsafe(priv->hw, skb);
+		spin_unlock_bh(&priv->ps_state_lock);
+	} else {
+		ieee80211_rx_irqsafe(priv->hw, skb);
+	}
+	*skb_p = NULL;
+
+	return;
+
+drop:
+	dev_warn(priv->hw_priv->pdev, "dropped received frame\n");
+	return;
+}
diff --git a/drivers/net/wireless/xradio/rx.h b/drivers/net/wireless/xradio/rx.h
new file mode 100644
index 0000000..0c02b23
--- /dev/null
+++ b/drivers/net/wireless/xradio/rx.h
@@ -0,0 +1,8 @@
+#ifndef XRADIO_RX_H
+#define XRADIO_RX_H
+
+void xradio_rx_cb(struct xradio_vif *priv,
+		  struct wsm_rx *arg,
+		  struct sk_buff **skb_p);
+
+#endif
diff --git a/drivers/net/wireless/xradio/scan.c b/drivers/net/wireless/xradio/scan.c
new file mode 100644
index 0000000..a8829c0
--- /dev/null
+++ b/drivers/net/wireless/xradio/scan.c
@@ -0,0 +1,897 @@
+/*
+ * Scan implementation for XRadio drivers
+ *
+ * Copyright (c) 2013, XRadio
+ * Author: XRadio
+ *
+ * This program is free software; you can 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/sched.h>
+#include "xradio.h"
+#include "scan.h"
+#include "sta.h"
+#include "pm.h"
+
+static void xradio_scan_restart_delayed(struct xradio_vif *priv);
+
+static void xradio_remove_wps_p2p_ie(struct wsm_template_frame *frame)
+{
+	u8 *ies;
+	u32 ies_len;
+	u32 ie_len;
+	u32 p2p_ie_len = 0;
+	u32 wps_ie_len = 0;
+
+
+	ies = &frame->skb->data[sizeof(struct ieee80211_hdr_3addr)];
+	ies_len = frame->skb->len - sizeof(struct ieee80211_hdr_3addr);
+	while (ies_len >= 6) {
+		ie_len = ies[1] + 2;
+		ies_len -= ie_len;
+		if ((ies[0] == WLAN_EID_VENDOR_SPECIFIC) && 
+			  (ies[2] == 0x00 && ies[3] == 0x50 && 
+			   ies[4] == 0xf2 && ies[5] == 0x04)) {
+			wps_ie_len = ie_len;
+			memmove(ies, ies + ie_len, ies_len);
+		} else if ((ies[0] == WLAN_EID_VENDOR_SPECIFIC) &&
+		           (ies[2] == 0x50 && ies[3] == 0x6f && 
+		            ies[4] == 0x9a && ies[5] == 0x09)) {
+			p2p_ie_len = ie_len;
+			memmove(ies, ies + ie_len, ies_len);
+		} else {
+			ies += ie_len;
+		}
+	}
+
+	if (p2p_ie_len || wps_ie_len) {
+		skb_trim(frame->skb, frame->skb->len - (p2p_ie_len + wps_ie_len));
+	}
+}
+
+static int xradio_scan_start(struct xradio_vif *priv, struct wsm_scan *scan)
+{
+	int ret, i;
+#ifdef FPGA_SETUP
+	int tmo = 5000;
+#else
+	int tmo = 5000;
+#endif
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+
+
+	for (i = 0; i < scan->numOfChannels; ++i)
+		tmo += scan->ch[i].maxChannelTime + 10;
+
+	atomic_set(&hw_priv->scan.in_progress, 1);
+	atomic_set(&hw_priv->recent_scan, 1);
+	xradio_pm_stay_awake(&hw_priv->pm_state, tmo * HZ / 1000);
+	ret = wsm_scan(hw_priv, scan, priv->if_id);
+	if (unlikely(ret)) {
+		scan_printk(XRADIO_DBG_WARN, "%s,wsm_scan failed!\n", __func__);
+		atomic_set(&hw_priv->scan.in_progress, 0);
+		xradio_scan_restart_delayed(priv);
+	} else {
+		queue_delayed_work(hw_priv->workqueue, &hw_priv->scan.timeout,
+						tmo * HZ / 1000);
+	}
+	return ret;
+}
+
+#ifdef ROAM_OFFLOAD
+static int xradio_sched_scan_start(struct xradio_vif *priv, struct wsm_scan *scan)
+{
+	int ret;
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+
+
+	ret = wsm_scan(hw_priv, scan, priv->if_id);
+	if (unlikely(ret)) {
+		atomic_set(&hw_priv->scan.in_progress, 0);
+		scan_printk(XRADIO_DBG_WARN,"%s,wsm_scan failed!\n", __func__);
+	}
+	return ret;
+}
+#endif /*ROAM_OFFLOAD*/
+
+int xradio_hw_scan(struct ieee80211_hw *hw,
+		   struct ieee80211_vif *vif,
+		   struct ieee80211_scan_request *hw_req)
+{
+	struct cfg80211_scan_request *req = &hw_req->req;
+	struct xradio_common *hw_priv = hw->priv;
+	struct xradio_vif *priv = xrwl_get_vif_from_ieee80211(vif);
+	struct wsm_template_frame frame = {
+		.frame_type = WSM_FRAME_TYPE_PROBE_REQUEST,
+	};
+	int i;
+
+	/* Scan when P2P_GO corrupt firmware MiniAP mode */
+	if (priv->join_status == XRADIO_JOIN_STATUS_AP) {
+		scan_printk(XRADIO_DBG_WARN,"%s, can't scan in AP mode!\n", __func__);
+		return -EOPNOTSUPP;
+	}
+
+	if (hw_priv->bh_error) {
+		scan_printk(XRADIO_DBG_NIY, "Ignoring scan bh error occur!\n");
+		return -EBUSY;
+	}
+
+	if (work_pending(&priv->offchannel_work) ||
+			(hw_priv->roc_if_id != -1)) {
+		scan_printk(XRADIO_DBG_WARN, "Offchannel work pending, "
+		            "ignoring scan work %d\n",  hw_priv->roc_if_id);
+		return -EBUSY;
+	}
+
+	if (req->n_ssids == 1 && !req->ssids[0].ssid_len)
+		req->n_ssids = 0;
+
+	scan_printk(XRADIO_DBG_NIY, "vif%d Scan request(%s-%dchs) for %d SSIDs.\n",
+	            priv->if_id, (req->channels[0]->band==NL80211_BAND_2GHZ)?"2.4G":"5G", 
+	            req->n_channels, req->n_ssids);
+	
+	/*delay multiple ssids scan of vif0 for 3s when connnetting to a node*/
+	if(hw_priv->connet_time[0] > 0 && req->n_ssids == 0 && priv->if_id == 0) {
+		long timeleft0 = hw_priv->connet_time[0] + SCAN_MAX_DELAY - jiffies;
+		if(jiffies >= hw_priv->connet_time[0] && timeleft0 > 0) {
+			scan_printk(XRADIO_DBG_NIY, "vif0 connetting, scan delay %ldms\n", 
+			            timeleft0*1000/HZ);
+			return -EBUSY;
+		}
+		hw_priv->connet_time[0] = 0;
+	}
+
+	if (req->n_ssids > hw->wiphy->max_scan_ssids){
+		scan_printk(XRADIO_DBG_ERROR, "%s: ssids is too much(%d)\n", 
+		            __func__, req->n_ssids);
+		return -EINVAL;
+	}
+
+	/* TODO by Icenowy: so strange function call */
+	frame.skb = ieee80211_probereq_get(hw, vif->addr, NULL, 0, 0);
+	if (!frame.skb) {
+		scan_printk(XRADIO_DBG_ERROR, "%s: ieee80211_probereq_get failed!\n", 
+		            __func__);
+		return -ENOMEM;
+	}
+
+#ifdef ROAM_OFFLOAD
+	if (priv->join_status != XRADIO_JOIN_STATUS_STA) {
+		if (req->channels[0]->band == NL80211_BAND_2GHZ)
+			hw_priv->num_scanchannels = 0;
+		else
+			hw_priv->num_scanchannels = hw_priv->num_2g_channels;
+
+		for (i=0; i < req->n_channels; i++) {
+			hw_priv->scan_channels[hw_priv->num_scanchannels + i].number = \
+				req->channels[i]->hw_value;
+			if (req->channels[i]->flags & IEEE80211_CHAN_NO_IR) {
+				hw_priv->scan_channels[hw_priv->num_scanchannels + i].minChannelTime = 50;
+				hw_priv->scan_channels[hw_priv->num_scanchannels + i].maxChannelTime = 110;
+			}
+			else {
+				hw_priv->scan_channels[hw_priv->num_scanchannels + i].minChannelTime = 10;
+				hw_priv->scan_channels[hw_priv->num_scanchannels + i].maxChannelTime = 40;
+				hw_priv->scan_channels[hw_priv->num_scanchannels + i].number |= \
+					XRADIO_SCAN_TYPE_ACTIVE;
+			}
+			hw_priv->scan_channels[hw_priv->num_scanchannels + i].txPowerLevel = \
+				req->channels[i]->max_power;
+			if (req->channels[0]->band == NL80211_BAND_5GHZ)
+				hw_priv->scan_channels[hw_priv->num_scanchannels + i].number |= \
+					XRADIO_SCAN_BAND_5G;
+		}
+		if (req->channels[0]->band == NL80211_BAND_2GHZ)
+			hw_priv->num_2g_channels = req->n_channels;
+		else
+			hw_priv->num_5g_channels = req->n_channels;
+	}
+	hw_priv->num_scanchannels = hw_priv->num_2g_channels + hw_priv->num_5g_channels;
+#endif /*ROAM_OFFLOAD*/
+
+	/* will be unlocked in xradio_scan_work() */
+	down(&hw_priv->scan.lock);
+	mutex_lock(&hw_priv->conf_mutex);
+
+		if (frame.skb) {
+			int ret = 0;
+			if (priv->if_id == 0)
+				xradio_remove_wps_p2p_ie(&frame);
+			ret = wsm_set_template_frame(hw_priv, &frame, priv->if_id);
+			if (ret) {
+				mutex_unlock(&hw_priv->conf_mutex);
+				up(&hw_priv->scan.lock);
+				dev_kfree_skb(frame.skb);
+				scan_printk(XRADIO_DBG_ERROR, "%s: wsm_set_template_frame failed: %d.\n",
+				             __func__, ret);
+				return ret;
+			}
+		}
+
+		wsm_vif_lock_tx(priv);
+
+		BUG_ON(hw_priv->scan.req);
+		hw_priv->scan.req     = req;
+		hw_priv->scan.n_ssids = 0;
+		hw_priv->scan.status  = 0;
+		hw_priv->scan.begin   = &req->channels[0];
+		hw_priv->scan.curr    = hw_priv->scan.begin;
+		hw_priv->scan.end     = &req->channels[req->n_channels];
+		hw_priv->scan.output_power = hw_priv->output_power;
+		hw_priv->scan.if_id = priv->if_id;
+		/* TODO:COMBO: Populate BIT4 in scanflags to decide on which MAC
+		 * address the SCAN request will be sent */
+
+		for (i = 0; i < req->n_ssids; ++i) {
+			struct wsm_ssid *dst = &hw_priv->scan.ssids[hw_priv->scan.n_ssids];
+			BUG_ON(req->ssids[i].ssid_len > sizeof(dst->ssid));
+			memcpy(&dst->ssid[0], req->ssids[i].ssid, sizeof(dst->ssid));
+			dst->length = req->ssids[i].ssid_len;
+			++hw_priv->scan.n_ssids;
+		}
+
+		mutex_unlock(&hw_priv->conf_mutex);
+
+		if (frame.skb)
+			dev_kfree_skb(frame.skb);
+		queue_work(hw_priv->workqueue, &hw_priv->scan.work);
+
+	return 0;
+}
+
+#ifdef ROAM_OFFLOAD
+int xradio_hw_sched_scan_start(struct ieee80211_hw *hw,
+		   struct ieee80211_vif *vif,
+		   struct cfg80211_sched_scan_request *req,
+		   struct ieee80211_sched_scan_ies *ies)
+{
+	struct xradio_common *hw_priv = hw->priv;
+	struct xradio_vif *priv = xrwl_get_vif_from_ieee80211(vif);
+	struct wsm_template_frame frame = {
+		.frame_type = WSM_FRAME_TYPE_PROBE_REQUEST,
+	};
+	int i;
+	
+
+	scan_printk(XRADIO_DBG_WARN, "Scheduled scan request-->.\n");
+	if (!priv->vif)
+		return -EINVAL;
+
+	/* Scan when P2P_GO corrupt firmware MiniAP mode */
+	if (priv->join_status == XRADIO_JOIN_STATUS_AP) {
+		scan_printk(XRADIO_DBG_WARN,"%s, can't scan in AP mode!\n", __func__);
+		return -EOPNOTSUPP;
+	}
+
+	scan_printk(XRADIO_DBG_WARN, "Scheduled scan: n_ssids %d, ssid[0].len = %d\n", 
+	            req->n_ssids, req->ssids[0].ssid_len);
+	if (req->n_ssids == 1 && !req->ssids[0].ssid_len)
+		req->n_ssids = 0;
+
+	scan_printk(XRADIO_DBG_NIY, "[SCAN] Scan request for %d SSIDs.\n", 
+	            req->n_ssids);
+
+	if (req->n_ssids > hw->wiphy->max_scan_ssids) [
+		scan_printk(XRADIO_DBG_ERROR, "%s: ssids is too much(%d)\n", 
+		            __func__, req->n_ssids);
+		return -EINVAL;
+	}
+
+	frame.skb = ieee80211_probereq_get(hw, priv->vif, NULL, 0,
+	                                   ies->ie[0], ies->len[0]);
+	if (!frame.skb) {
+		scan_printk(XRADIO_DBG_ERROR, "%s: ieee80211_probereq_get failed!\n", 
+		            __func__);
+		return -ENOMEM;
+	}
+
+	/* will be unlocked in xradio_scan_work() */
+	down(&hw_priv->scan.lock);
+	mutex_lock(&hw_priv->conf_mutex);
+	if (frame.skb) {
+		int ret;
+		if (priv->if_id == 0)
+			xradio_remove_wps_p2p_ie(&frame);
+		ret = wsm_set_template_frame(hw_priv, &frame, priv->if_id);
+		if (0 == ret) {
+			/* Host want to be the probe responder. */
+			ret = wsm_set_probe_responder(priv, true);
+		}
+		if (ret) {
+			mutex_unlock(&hw_priv->conf_mutex);
+			up(&hw_priv->scan.lock);
+			dev_kfree_skb(frame.skb);
+			scan_printk(XRADIO_DBG_ERROR, "%s: wsm_set_probe_responder failed: %d.\n",
+				             __func__, ret);
+			return ret;
+		}
+	}
+
+	wsm_lock_tx(hw_priv);
+	BUG_ON(hw_priv->scan.req);
+	hw_priv->scan.sched_req = req;
+	hw_priv->scan.n_ssids = 0;
+	hw_priv->scan.status = 0;
+	hw_priv->scan.begin = &req->channels[0];
+	hw_priv->scan.curr = hw_priv->scan.begin;
+	hw_priv->scan.end = &req->channels[req->n_channels];
+	hw_priv->scan.output_power = hw_priv->output_power;
+
+	for (i = 0; i < req->n_ssids; ++i) {
+		u8 j;
+		struct wsm_ssid *dst = &hw_priv->scan.ssids[hw_priv->scan.n_ssids];
+		BUG_ON(req->ssids[i].ssid_len > sizeof(dst->ssid));
+		memcpy(&dst->ssid[0], req->ssids[i].ssid, sizeof(dst->ssid));
+		dst->length = req->ssids[i].ssid_len;
+		++hw_priv->scan.n_ssids;
+		scan_printk(XRADIO_DBG_NIY, "SSID %d\n",i);
+		for(j=0; j<req->ssids[i].ssid_len; j++)
+			scan_printk(XRADIO_DBG_NIY, "0x%x\n", req->ssids[i].ssid[j]);
+	}
+	mutex_unlock(&hw_priv->conf_mutex);
+
+	if (frame.skb)
+		dev_kfree_skb(frame.skb);
+	queue_work(hw_priv->workqueue, &hw_priv->scan.swork);
+	scan_printk(XRADIO_DBG_NIY, "<-- Scheduled scan request.\n");
+	return 0;
+}
+#endif /*ROAM_OFFLOAD*/
+
+void xradio_scan_work(struct work_struct *work)
+{
+	struct xradio_common *hw_priv = container_of(work,
+						struct xradio_common,
+						scan.work);
+	struct xradio_vif *priv;
+	struct ieee80211_channel **it;
+	struct wsm_scan scan = {
+		.scanType = WSM_SCAN_TYPE_FOREGROUND,
+		.scanFlags = 0, /* TODO:COMBO */
+		//.scanFlags = WSM_SCAN_FLAG_SPLIT_METHOD, /* TODO:COMBO */
+	};
+	bool first_run;
+	int i;
+	const u32 ProbeRequestTime  = 2;
+	const u32 ChannelRemainTime = 15;
+	u32 maxChannelTime;
+	struct cfg80211_scan_info scan_info;
+
+
+	priv = __xrwl_hwpriv_to_vifpriv(hw_priv, hw_priv->scan.if_id);
+
+	/*TODO: COMBO: introduce locking so vif is not removed in meanwhile */
+	if (!priv) {
+		scan_printk(XRADIO_DBG_WARN, "interface removed, "
+		            "ignoring scan work\n");
+		return;
+	}
+
+	if (priv->if_id)
+		scan.scanFlags |= WSM_FLAG_MAC_INSTANCE_1;
+	else
+		scan.scanFlags &= ~WSM_FLAG_MAC_INSTANCE_1;
+
+	/* No need to set WSM_SCAN_FLAG_FORCE_BACKGROUND in BSS_LOSS work. 
+	 * yangfh 2015-11-11 18:45:02 */
+	//xradio_for_each_vif(hw_priv, vif, i) {
+	//	if (!vif)
+	//		continue;
+	//	if (vif->bss_loss_status > XRADIO_BSS_LOSS_NONE)
+	//		scan.scanFlags |= WSM_SCAN_FLAG_FORCE_BACKGROUND;
+	//}
+
+	first_run = (hw_priv->scan.begin == hw_priv->scan.curr &&
+	             hw_priv->scan.begin != hw_priv->scan.end);
+	if (first_run) {
+		/* Firmware gets crazy if scan request is sent
+		 * when STA is joined but not yet associated.
+		 * Force unjoin in this case. */
+		if (cancel_delayed_work_sync(&priv->join_timeout) > 0)
+			xradio_join_timeout(&priv->join_timeout.work);
+	}
+
+	mutex_lock(&hw_priv->conf_mutex);
+	if (first_run) {
+#if 0
+			if (priv->join_status == XRADIO_JOIN_STATUS_STA &&
+			    !(priv->powersave_mode.pmMode & WSM_PSM_PS)) {
+				struct wsm_set_pm pm = priv->powersave_mode;
+				pm.pmMode = WSM_PSM_PS;
+				xradio_set_pm(priv, &pm);
+			} else
+#endif
+			if (priv->join_status == XRADIO_JOIN_STATUS_MONITOR) {
+				/* FW bug: driver has to restart p2p-dev mode
+				 * after scan */
+				xradio_disable_listening(priv);
+			}
+	}
+
+	if (!hw_priv->scan.req || (hw_priv->scan.curr == hw_priv->scan.end)) {
+			if (hw_priv->scan.output_power != hw_priv->output_power) {
+			/* TODO:COMBO: Change when mac80211 implementation
+			 * is available for output power also */
+					WARN_ON(wsm_set_output_power(hw_priv, hw_priv->output_power * 10,
+					                             priv->if_id));
+			}
+
+#if 0
+		if (priv->join_status == XRADIO_JOIN_STATUS_STA &&
+		    !(priv->powersave_mode.pmMode & WSM_PSM_PS))
+			xradio_set_pm(priv, &priv->powersave_mode);
+#endif
+
+		if (hw_priv->scan.status < 0)
+			scan_printk(XRADIO_DBG_ERROR, "Scan failed (%d).\n", hw_priv->scan.status);
+		else if (hw_priv->scan.req)
+			scan_printk(XRADIO_DBG_NIY, "Scan completed.\n");
+		else
+			scan_printk(XRADIO_DBG_NIY, "Scan canceled.\n");
+
+		hw_priv->scan.req = NULL;
+		xradio_scan_restart_delayed(priv);
+		wsm_unlock_tx(hw_priv);
+		mutex_unlock(&hw_priv->conf_mutex);
+		memset(&scan_info, 0, sizeof(scan_info));
+		scan_info.aborted = hw_priv->scan.status ? 1 : 0;
+		ieee80211_scan_completed(hw_priv->hw, &scan_info);
+		up(&hw_priv->scan.lock);
+		return;
+
+	} else {
+		struct ieee80211_channel *first = *hw_priv->scan.curr;
+		for (it = hw_priv->scan.curr + 1, i = 1;
+		     it != hw_priv->scan.end && i < WSM_SCAN_MAX_NUM_OF_CHANNELS;
+		     ++it, ++i) {
+			if ((*it)->band != first->band)
+				break;
+			if (((*it)->flags ^ first->flags) & IEEE80211_CHAN_NO_IR)
+				break;
+			if (!(first->flags & IEEE80211_CHAN_NO_IR) &&
+			    (*it)->max_power != first->max_power)
+				break;
+		}
+		scan.band = first->band;
+
+		if (hw_priv->scan.req->no_cck)
+			scan.maxTransmitRate = WSM_TRANSMIT_RATE_6;
+		else
+			scan.maxTransmitRate = WSM_TRANSMIT_RATE_1;
+
+		/* TODO: Is it optimal? */
+		scan.numOfProbeRequests = (first->flags & IEEE80211_CHAN_NO_IR) ? 0 : 2;
+
+		scan.numOfSSIDs = hw_priv->scan.n_ssids;
+		scan.ssids = &hw_priv->scan.ssids[0];
+		scan.numOfChannels = it - hw_priv->scan.curr;
+		/* TODO: Is it optimal? */
+		scan.probeDelay = 100;
+		/* It is not stated in WSM specification, however
+		 * FW team says that driver may not use FG scan
+		 * when joined. */
+		if (priv->join_status == XRADIO_JOIN_STATUS_STA) {
+			scan.scanType = WSM_SCAN_TYPE_BACKGROUND;
+			scan.scanFlags = WSM_SCAN_FLAG_FORCE_BACKGROUND;
+		}
+		scan.ch = kzalloc(sizeof(struct wsm_scan_ch[it - hw_priv->scan.curr]), GFP_KERNEL);
+		if (!scan.ch) {
+			hw_priv->scan.status = -ENOMEM;
+			scan_printk(XRADIO_DBG_ERROR, "xr_kzalloc wsm_scan_ch failed.\n");
+			goto fail;
+		}
+		maxChannelTime = (scan.numOfSSIDs * scan.numOfProbeRequests *ProbeRequestTime) + 
+		                  ChannelRemainTime;
+		maxChannelTime = (maxChannelTime < 35) ? 35 : maxChannelTime;
+		for (i = 0; i < scan.numOfChannels; ++i) {
+			scan.ch[i].number = hw_priv->scan.curr[i]->hw_value;
+
+
+				if (hw_priv->scan.curr[i]->flags & IEEE80211_CHAN_NO_IR) {
+					scan.ch[i].minChannelTime = 50;
+					scan.ch[i].maxChannelTime = 110;
+				} else {
+					scan.ch[i].minChannelTime = 15;
+					scan.ch[i].maxChannelTime = maxChannelTime;
+				}
+
+
+		}
+
+			if (!(first->flags & IEEE80211_CHAN_NO_IR) &&
+			    hw_priv->scan.output_power != first->max_power) {
+			    hw_priv->scan.output_power = first->max_power;
+				/* TODO:COMBO: Change after mac80211 implementation
+				* complete */
+				WARN_ON(wsm_set_output_power(hw_priv, hw_priv->scan.output_power * 10,
+				                             priv->if_id));
+			}
+
+			down(&hw_priv->scan.status_lock);
+			hw_priv->scan.status = xradio_scan_start(priv, &scan);
+
+		kfree(scan.ch);
+		if (WARN_ON(hw_priv->scan.status)) {
+			scan_printk(XRADIO_DBG_ERROR, "scan failed, status=%d.\n",
+			            hw_priv->scan.status);
+			up(&hw_priv->scan.status_lock);
+			goto fail;
+		}
+		up(&hw_priv->scan.status_lock);
+		hw_priv->scan.curr = it;
+	}
+	mutex_unlock(&hw_priv->conf_mutex);
+	return;
+
+fail:
+	hw_priv->scan.curr = hw_priv->scan.end;
+	mutex_unlock(&hw_priv->conf_mutex);
+	queue_work(hw_priv->workqueue, &hw_priv->scan.work);
+	return;
+}
+
+#ifdef ROAM_OFFLOAD
+void xradio_sched_scan_work(struct work_struct *work)
+{
+	struct xradio_common *hw_priv = container_of(work, struct xradio_common,
+		scan.swork);
+	struct wsm_scan scan;
+	struct wsm_ssid scan_ssid;
+	int i;
+	struct xradio_vif *priv = NULL;
+
+
+	priv = xrwl_hwpriv_to_vifpriv(hw_priv, hw_priv->scan.if_id);
+	if (unlikely(!priv)) {
+		WARN_ON(1);
+		return;
+	}
+
+	spin_unlock(&priv->vif_lock);
+	/* Firmware gets crazy if scan request is sent
+	 * when STA is joined but not yet associated.
+	 * Force unjoin in this case. */
+	if (cancel_delayed_work_sync(&priv->join_timeout) > 0) {
+		xradio_join_timeout(&priv->join_timeout.work);
+	}
+	mutex_lock(&hw_priv->conf_mutex);
+	hw_priv->auto_scanning = 1;
+	scan.band = 0;
+
+	if (priv->join_status == XRADIO_JOIN_STATUS_STA)
+		scan.scanType = 3; /* auto background */
+	else
+		scan.scanType = 2; /* auto foreground */
+
+	scan.scanFlags = 0x01; /* bit 0 set => forced background scan */
+	scan.maxTransmitRate = WSM_TRANSMIT_RATE_6;
+	scan.autoScanInterval = (0xba << 24)|(30 * 1024); /* 30 seconds, -70 rssi */
+	scan.numOfProbeRequests = 1;
+	//scan.numOfChannels = 11;
+	scan.numOfChannels = hw_priv->num_scanchannels;
+	scan.numOfSSIDs = 1;
+	scan.probeDelay = 100;
+	scan_ssid.length = priv->ssid_length;
+	memcpy(scan_ssid.ssid, priv->ssid, priv->ssid_length);
+	scan.ssids = &scan_ssid;
+
+	scan.ch = xr_kzalloc(sizeof(struct wsm_scan_ch[scan.numOfChannels]), false);
+	if (!scan.ch) {
+		scan_printk(XRADIO_DBG_ERROR, "xr_kzalloc wsm_scan_ch failed.\n");
+		hw_priv->scan.status = -ENOMEM;
+		goto fail;
+	}
+
+	for (i = 0; i < scan.numOfChannels; i++) {
+		scan.ch[i].number = hw_priv->scan_channels[i].number;
+		scan.ch[i].minChannelTime = hw_priv->scan_channels[i].minChannelTime;
+		scan.ch[i].maxChannelTime = hw_priv->scan_channels[i].maxChannelTime;
+		scan.ch[i].txPowerLevel = hw_priv->scan_channels[i].txPowerLevel;
+	}
+
+#if 0
+	for (i = 1; i <= scan.numOfChannels; i++) {
+		scan.ch[i-1].number = i;
+		scan.ch[i-1].minChannelTime = 10;
+		scan.ch[i-1].maxChannelTime = 40;
+	}
+#endif
+
+	hw_priv->scan.status = xradio_sched_scan_start(priv, &scan);
+	kfree(scan.ch);
+	if (hw_priv->scan.status) {
+		scan_printk(XRADIO_DBG_ERROR, "scan failed, status=%d.\n",
+			            hw_priv->scan.status);
+		goto fail;
+	}
+	mutex_unlock(&hw_priv->conf_mutex);
+	return;
+
+fail:
+	mutex_unlock(&hw_priv->conf_mutex);
+	queue_work(hw_priv->workqueue, &hw_priv->scan.swork);
+	return;
+}
+
+void xradio_hw_sched_scan_stop(struct xradio_common *hw_priv)
+{
+	struct xradio_vif *priv = NULL;
+
+	priv = xrwl_hwpriv_to_vifpriv(hw_priv,hw_priv->scan.if_id);
+	if (unlikely(!priv))
+		return;
+
+	spin_unlock(&priv->vif_lock);
+	wsm_stop_scan(hw_priv, priv->if_id);
+
+	return;
+}
+#endif /*ROAM_OFFLOAD*/
+
+
+static void xradio_scan_restart_delayed(struct xradio_vif *priv)
+{
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+
+
+	if (priv->delayed_link_loss) {
+		int tmo = hw_priv->scan.direct_probe ? 0 : priv->cqm_beacon_loss_count;
+
+		priv->delayed_link_loss = 0;
+		/* Restart beacon loss timer and requeue
+		   BSS loss work. */
+		scan_printk(XRADIO_DBG_WARN, "[CQM] Requeue BSS loss in %d "
+		           "beacons.\n", tmo);
+		cancel_delayed_work_sync(&priv->bss_loss_work);
+		queue_delayed_work(hw_priv->workqueue, &priv->bss_loss_work,
+		                   tmo * HZ / 10);
+		
+	}
+
+	/* FW bug: driver has to restart p2p-dev mode after scan. */
+	if (priv->join_status == XRADIO_JOIN_STATUS_MONITOR) {
+		/*xradio_enable_listening(priv);*/
+		WARN_ON(1);
+		xradio_update_filtering(priv);
+		scan_printk(XRADIO_DBG_WARN, "driver has to restart "
+		            "p2p-dev mode after scan");
+	}
+
+	if (atomic_xchg(&priv->delayed_unjoin, 0)) {
+		if (queue_work(hw_priv->workqueue, &priv->unjoin_work) <= 0)
+			wsm_unlock_tx(hw_priv);
+	}
+}
+
+static void xradio_scan_complete(struct xradio_common *hw_priv, int if_id)
+{
+	struct xradio_vif *priv;
+	atomic_xchg(&hw_priv->recent_scan, 0);
+
+
+	if (hw_priv->scan.direct_probe) {
+		mutex_lock(&hw_priv->conf_mutex);
+		priv = __xrwl_hwpriv_to_vifpriv(hw_priv, if_id);
+		if (priv) {
+			scan_printk(XRADIO_DBG_MSG, "Direct probe complete.\n");
+			xradio_scan_restart_delayed(priv);
+		} else {
+			scan_printk(XRADIO_DBG_MSG, "Direct probe complete without interface!\n");
+		}
+		mutex_unlock(&hw_priv->conf_mutex);
+		hw_priv->scan.direct_probe = 0;
+		up(&hw_priv->scan.lock);
+		wsm_unlock_tx(hw_priv);
+	} else {
+		xradio_scan_work(&hw_priv->scan.work);
+	}
+}
+
+void xradio_scan_complete_cb(struct xradio_common *hw_priv,
+                             struct wsm_scan_complete *arg)
+{
+	struct xradio_vif *priv = xrwl_hwpriv_to_vifpriv(hw_priv,
+					hw_priv->scan.if_id);
+
+
+	if (unlikely(!priv))
+		return;
+
+#ifdef ROAM_OFFLOAD
+	if (hw_priv->auto_scanning)
+		queue_delayed_work(hw_priv->workqueue, &hw_priv->scan.timeout, 0);
+#endif /*ROAM_OFFLOAD*/
+
+	if (unlikely(priv->mode == NL80211_IFTYPE_UNSPECIFIED)) {
+		/* STA is stopped. */
+		spin_unlock(&priv->vif_lock);
+		scan_printk(XRADIO_DBG_WARN, "%s: priv->mode UNSPECIFIED.\n", __func__);
+		return;
+	}
+	spin_unlock(&priv->vif_lock);
+
+	/*
+	if(hw_priv->scan.status == -ETIMEDOUT)
+		scan_printk(XRADIO_DBG_WARN, "Scan timeout already occured. "
+		            "Don't cancel work");
+	if ((hw_priv->scan.status != -ETIMEDOUT) &&
+		(cancel_delayed_work_sync(&hw_priv->scan.timeout) > 0)) {
+		hw_priv->scan.status = 1;
+		queue_delayed_work(hw_priv->workqueue, &hw_priv->scan.timeout, 0);
+	}
+	*/  // should not involve status as a condition
+
+	if (cancel_delayed_work_sync(&hw_priv->scan.timeout) > 0) {
+		down(&hw_priv->scan.status_lock);
+		hw_priv->scan.status = 1;
+		up(&hw_priv->scan.status_lock);
+		queue_delayed_work(hw_priv->workqueue, &hw_priv->scan.timeout, 0);
+	}
+}
+
+void xradio_scan_timeout(struct work_struct *work)
+{
+	struct xradio_common *hw_priv =
+		container_of(work, struct xradio_common, scan.timeout.work);
+
+
+	if (likely(atomic_xchg(&hw_priv->scan.in_progress, 0))) {
+		if (hw_priv->scan.status > 0)
+			hw_priv->scan.status = 0;
+		else if (!hw_priv->scan.status) {
+			scan_printk(XRADIO_DBG_WARN, "Timeout waiting for scan "
+			            "complete notification.\n");
+			hw_priv->scan.status = -ETIMEDOUT;
+			hw_priv->scan.curr   = hw_priv->scan.end;
+			WARN_ON(wsm_stop_scan(hw_priv, hw_priv->scan.if_id ? 1 : 0));
+		}
+		xradio_scan_complete(hw_priv, hw_priv->scan.if_id);
+#ifdef ROAM_OFFLOAD
+	} else if (hw_priv->auto_scanning) {
+		hw_priv->auto_scanning = 0;
+		ieee80211_sched_scan_results(hw_priv->hw);
+#endif /*ROAM_OFFLOAD*/
+	}
+}
+
+void xradio_probe_work(struct work_struct *work)
+{
+	struct xradio_common *hw_priv =
+		container_of(work, struct xradio_common, scan.probe_work.work);
+	struct xradio_vif *priv;
+	u8 queueId = xradio_queue_get_queue_id(hw_priv->pending_frame_id);
+	struct xradio_queue *queue = &hw_priv->tx_queue[queueId];
+	const struct xradio_txpriv *txpriv;
+	struct wsm_tx *wsm;
+	struct wsm_template_frame frame = {
+		.frame_type = WSM_FRAME_TYPE_PROBE_REQUEST,
+	};
+	struct wsm_ssid ssids[1] = {{
+		.length = 0,
+	} };
+	struct wsm_scan_ch ch[1] = {{
+		.minChannelTime = 0,
+		.maxChannelTime = 10,
+	} };
+	struct wsm_scan scan = {
+		.scanType = WSM_SCAN_TYPE_FOREGROUND,
+		.numOfProbeRequests = 1,
+		.probeDelay = 0,
+		.numOfChannels = 1,
+		.ssids = ssids,
+		.ch = ch,
+	};
+	u8 *ies;
+	size_t ies_len;
+	int ret = 1;
+	scan_printk(XRADIO_DBG_MSG, "%s:Direct probe.\n", __func__);
+
+	BUG_ON(queueId >= 4);
+	BUG_ON(!hw_priv->channel);
+
+	mutex_lock(&hw_priv->conf_mutex);
+	if (unlikely(down_trylock(&hw_priv->scan.lock))) {
+		/* Scan is already in progress. Requeue self. */
+		schedule();
+		queue_delayed_work(hw_priv->workqueue, &hw_priv->scan.probe_work, 
+		                   HZ / 10);
+		mutex_unlock(&hw_priv->conf_mutex);
+		return;
+	}
+
+	if (xradio_queue_get_skb(queue,	hw_priv->pending_frame_id, &frame.skb, &txpriv)) {
+		up(&hw_priv->scan.lock);
+		mutex_unlock(&hw_priv->conf_mutex);
+		wsm_unlock_tx(hw_priv);
+		scan_printk(XRADIO_DBG_ERROR, "%s:xradio_queue_get_skb error!\n", __func__);
+		return;
+	}
+	priv = __xrwl_hwpriv_to_vifpriv(hw_priv, txpriv->if_id);
+	if (!priv) {
+		up(&hw_priv->scan.lock);
+		mutex_unlock(&hw_priv->conf_mutex);
+		scan_printk(XRADIO_DBG_ERROR, "%s:priv error!\n", __func__);
+		return;
+	}
+	wsm = (struct wsm_tx *)frame.skb->data;
+	scan.maxTransmitRate = wsm->maxTxRate;
+	scan.band = (hw_priv->channel->band == NL80211_BAND_5GHZ) ?
+	             WSM_PHY_BAND_5G : WSM_PHY_BAND_2_4G;
+	if (priv->join_status == XRADIO_JOIN_STATUS_STA) {
+		scan.scanType  = WSM_SCAN_TYPE_BACKGROUND;
+		scan.scanFlags = WSM_SCAN_FLAG_FORCE_BACKGROUND;
+		if (priv->if_id)
+			scan.scanFlags |= WSM_FLAG_MAC_INSTANCE_1;
+		else
+			scan.scanFlags &= ~WSM_FLAG_MAC_INSTANCE_1;
+	}
+
+	/* No need to set WSM_SCAN_FLAG_FORCE_BACKGROUND in BSS_LOSS work. 
+	 * yangfh 2015-11-11 18:45:02 */
+	//xradio_for_each_vif(hw_priv, vif, i) {
+	//	if (!vif)
+	//		continue;
+	//	if (vif->bss_loss_status > XRADIO_BSS_LOSS_NONE)
+	//		scan.scanFlags |= WSM_SCAN_FLAG_FORCE_BACKGROUND;
+	//}
+	
+	ch[0].number = hw_priv->channel->hw_value;
+	skb_pull(frame.skb, txpriv->offset);
+	ies = &frame.skb->data[sizeof(struct ieee80211_hdr_3addr)];
+	ies_len = frame.skb->len - sizeof(struct ieee80211_hdr_3addr);
+
+	if (ies_len) {
+		u8 *ssidie = (u8 *)cfg80211_find_ie(WLAN_EID_SSID, ies, ies_len);
+		if (ssidie && ssidie[1] && ssidie[1] <= sizeof(ssids[0].ssid)) {
+			u8 *nextie = &ssidie[2 + ssidie[1]];
+			/* Remove SSID from the IE list. It has to be provided
+			 * as a separate argument in xradio_scan_start call */
+
+			/* Store SSID localy */
+			ssids[0].length = ssidie[1];
+			memcpy(ssids[0].ssid, &ssidie[2], ssids[0].length);
+			scan.numOfSSIDs = 1;
+
+			/* Remove SSID from IE list */
+			ssidie[1] = 0;
+			memmove(&ssidie[2], nextie, &ies[ies_len] - nextie);
+			skb_trim(frame.skb, frame.skb->len - ssids[0].length);
+		}
+	}
+
+	if (priv->if_id == 0)
+		xradio_remove_wps_p2p_ie(&frame);
+
+	/* FW bug: driver has to restart p2p-dev mode after scan */
+	if (priv->join_status == XRADIO_JOIN_STATUS_MONITOR) {
+		WARN_ON(1);
+		/*xradio_disable_listening(priv);*/
+	}
+	ret = WARN_ON(wsm_set_template_frame(hw_priv, &frame,
+				priv->if_id));
+
+	hw_priv->scan.direct_probe = 1;
+	hw_priv->scan.if_id = priv->if_id;
+	if (!ret) {
+		wsm_flush_tx(hw_priv);
+		ret = WARN_ON(xradio_scan_start(priv, &scan));
+	}
+	mutex_unlock(&hw_priv->conf_mutex);
+
+	skb_push(frame.skb, txpriv->offset);
+	if (!ret)
+		IEEE80211_SKB_CB(frame.skb)->flags |= IEEE80211_TX_STAT_ACK;
+
+		BUG_ON(xradio_queue_remove(queue, hw_priv->pending_frame_id));
+
+	if (ret) {
+		hw_priv->scan.direct_probe = 0;
+		up(&hw_priv->scan.lock);
+		wsm_unlock_tx(hw_priv);
+	}
+
+	return;
+}
diff --git a/drivers/net/wireless/xradio/scan.h b/drivers/net/wireless/xradio/scan.h
new file mode 100644
index 0000000..5c520ed
--- /dev/null
+++ b/drivers/net/wireless/xradio/scan.h
@@ -0,0 +1,71 @@
+/*
+ * Scan interfaces for XRadio drivers
+ *
+ * Copyright (c) 2013, XRadio
+ * Author: XRadio
+ *
+ * This program is free software; you can 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 SCAN_H_INCLUDED
+#define SCAN_H_INCLUDED
+
+#include <linux/semaphore.h>
+#include "wsm.h"
+
+/* external */ struct sk_buff;
+/* external */ struct cfg80211_scan_request;
+/* external */ struct ieee80211_channel;
+/* external */ struct ieee80211_hw;
+/* external */ struct work_struct;
+
+#define SCAN_MAX_DELAY      (3*HZ)   //3s, add by yangfh for connect
+
+struct xradio_scan {
+	struct semaphore lock;
+	struct work_struct work;
+#ifdef ROAM_OFFLOAD
+	struct work_struct swork; /* scheduled scan work */
+	struct cfg80211_sched_scan_request *sched_req;
+#endif /*ROAM_OFFLOAD*/
+	struct delayed_work timeout;
+	struct cfg80211_scan_request *req;
+	struct ieee80211_channel **begin;
+	struct ieee80211_channel **curr;
+	struct ieee80211_channel **end;
+	struct wsm_ssid ssids[WSM_SCAN_MAX_NUM_OF_SSIDS];
+	int output_power;
+	int n_ssids;
+	//add by liwei, for h64 ping WS550 BUG
+	struct semaphore status_lock;
+	int status;
+	atomic_t in_progress;
+	/* Direct probe requests workaround */
+	struct delayed_work probe_work;
+	int direct_probe;
+	u8 if_id;
+};
+
+int xradio_hw_scan(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
+                   struct ieee80211_scan_request *req);
+#ifdef ROAM_OFFLOAD
+int xradio_hw_sched_scan_start(struct ieee80211_hw *hw,
+                               struct ieee80211_vif *vif,
+                               struct cfg80211_sched_scan_request *req,
+                               struct ieee80211_sched_scan_ies *ies);
+void xradio_hw_sched_scan_stop(struct xradio_common *priv);
+void xradio_sched_scan_work(struct work_struct *work);
+#endif /*ROAM_OFFLOAD*/
+void xradio_scan_work(struct work_struct *work);
+void xradio_scan_timeout(struct work_struct *work);
+void xradio_scan_complete_cb(struct xradio_common *priv,
+                             struct wsm_scan_complete *arg);
+
+/* ******************************************************************** */
+/* Raw probe requests TX workaround					*/
+void xradio_probe_work(struct work_struct *work);
+
+#endif
diff --git a/drivers/net/wireless/xradio/sdio.c b/drivers/net/wireless/xradio/sdio.c
new file mode 100644
index 0000000..13d4eb5
--- /dev/null
+++ b/drivers/net/wireless/xradio/sdio.c
@@ -0,0 +1,246 @@
+/*
+ * SDIO driver for XRadio drivers
+ *
+ * Copyright (c) 2013, XRadio
+ * Author: XRadio
+ *
+ * This program is free software; you can 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/mmc/host.h>
+#include <linux/mmc/sdio_func.h>
+#include <linux/mmc/card.h>
+#include <linux/mmc/sdio.h>
+#include <asm/mach-types.h>
+#include <linux/of.h>
+#include <linux/of_irq.h>
+#include <linux/interrupt.h>
+
+#include "xradio.h"
+#include "sdio.h"
+#include "main.h"
+
+/* sdio vendor id and device id*/
+#define SDIO_VENDOR_ID_XRADIO 0x0020
+#define SDIO_DEVICE_ID_XRADIO 0x2281
+static const struct sdio_device_id xradio_sdio_ids[] = {
+	{ SDIO_DEVICE(SDIO_VENDOR_ID_XRADIO, SDIO_DEVICE_ID_XRADIO) },
+	//{ SDIO_DEVICE(SDIO_ANY_ID, SDIO_ANY_ID) },
+	{ /* end: all zeroes */			},
+};
+
+/* sbus_ops implemetation */
+int sdio_data_read(struct xradio_common* self, unsigned int addr,
+                          void *dst, int count)
+{
+	int ret = sdio_memcpy_fromio(self->sdio_func, dst, addr, count);
+//	printk("sdio_memcpy_fromio 0x%x:%d ret %d\n", addr, count, ret);
+//	print_hex_dump_bytes("sdio read ", 0, dst, min(count,32));
+	return ret;
+}
+
+int sdio_data_write(struct xradio_common* self, unsigned int addr,
+                           const void *src, int count)
+{
+	int ret = sdio_memcpy_toio(self->sdio_func, addr, (void *)src, count);
+//	printk("sdio_memcpy_toio 0x%x:%d ret %d\n", addr, count, ret);
+//	print_hex_dump_bytes("sdio write", 0, src, min(count,32));
+	return ret;
+}
+
+void sdio_lock(struct xradio_common* self)
+{
+	sdio_claim_host(self->sdio_func);
+}
+
+void sdio_unlock(struct xradio_common *self)
+{
+	sdio_release_host(self->sdio_func);
+}
+
+size_t sdio_align_len(struct xradio_common *self, size_t size)
+{
+	return sdio_align_size(self->sdio_func, size);
+}
+
+int sdio_set_blk_size(struct xradio_common *self, size_t size)
+{
+	return sdio_set_block_size(self->sdio_func, size);
+}
+
+extern void xradio_irq_handler(struct xradio_common*);
+
+static irqreturn_t sdio_irq_handler(int irq, void *dev_id)
+{
+	struct sdio_func *func = (struct sdio_func*) dev_id;
+	struct xradio_common *self = sdio_get_drvdata(func);
+	if (self != NULL)
+		xradio_irq_handler(self);
+	return IRQ_HANDLED;
+}
+
+static int sdio_enableint(struct sdio_func* func)
+{
+	int ret = 0;
+	u8 cccr;
+	int func_num;
+
+	sdio_claim_host(func);
+
+	/* Hack to access Fuction-0 */
+	func_num = func->num;
+	func->num = 0;
+	cccr = sdio_readb(func, SDIO_CCCR_IENx, &ret);
+	cccr |= BIT(0); /* Master interrupt enable ... */
+	cccr |= BIT(func_num); /* ... for our function */
+	sdio_writeb(func, cccr, SDIO_CCCR_IENx, &ret);
+
+	/* Restore the WLAN function number */
+	func->num = func_num;
+
+	sdio_release_host(func);
+
+	return ret;
+}
+
+int sdio_pm(struct xradio_common *self, bool  suspend)
+{
+	int ret = 0;
+	if (suspend) {
+		/* Notify SDIO that XRADIO will remain powered during suspend */
+		ret = sdio_set_host_pm_flags(self->sdio_func, MMC_PM_KEEP_POWER);
+		if (ret)
+			dev_dbg(&self->sdio_func->dev, "Error setting SDIO pm flags: %i\n", ret);
+	}
+
+	return ret;
+}
+
+static const struct of_device_id xradio_sdio_of_match_table[] = {
+	{ .compatible = "xradio,xr819" },
+	{ }
+};
+
+static int xradio_probe_of(struct sdio_func *func)
+{
+	struct device *dev = &func->dev;
+	struct device_node *np = dev->of_node;
+	const struct of_device_id *of_id;
+	int irq;
+	int ret;
+
+	of_id = of_match_node(xradio_sdio_of_match_table, np);
+	if (!of_id)
+		return -ENODEV;
+
+	//pdev_data->family = of_id->data;
+
+	irq = irq_of_parse_and_map(np, 0);
+	if (!irq) {
+		dev_err(dev, "No irq in platform data\n");
+		return -EINVAL;
+	}
+
+	ret = devm_request_irq(dev, irq, sdio_irq_handler, 0, "xradio", func);
+	if (ret) {
+		dev_err(dev, "Failed to request irq_wakeup.\n");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+/* Probe Function to be called by SDIO stack when device is discovered */
+static int sdio_probe(struct sdio_func *func,
+                      const struct sdio_device_id *id)
+{
+	dev_dbg(&func->dev, "XRadio Device:sdio clk=%d\n",
+	            func->card->host->ios.clock);
+	dev_dbg(&func->dev, "sdio func->class=%x\n", func->class);
+	dev_dbg(&func->dev, "sdio_vendor: 0x%04x\n", func->vendor);
+	dev_dbg(&func->dev, "sdio_device: 0x%04x\n", func->device);
+	dev_dbg(&func->dev, "Function#: 0x%04x\n",   func->num);
+
+#if 0  //for odly and sdly debug.
+{
+	u32 sdio_param = 0;
+	sdio_param = readl(__io_address(0x01c20088));
+	sdio_param &= ~(0xf<<8);
+	sdio_param |= 3<<8;
+	sdio_param &= ~(0xf<<20);
+	sdio_param |= s_dly<<20;
+	writel(sdio_param, __io_address(0x01c20088));
+	sbus_printk(XRADIO_DBG_ALWY, "%s: 0x01c20088=0x%08x\n", __func__, sdio_param);
+}
+#endif
+
+	xradio_probe_of(func);
+
+	func->card->quirks |= MMC_QUIRK_BROKEN_BYTE_MODE_512;
+	sdio_claim_host(func);
+	sdio_enable_func(func);
+	sdio_release_host(func);
+
+	sdio_enableint(func);
+
+	xradio_core_init(func);
+
+	try_module_get(func->dev.driver->owner);
+	return 0;
+}
+/* Disconnect Function to be called by SDIO stack when
+ * device is disconnected */
+static void sdio_remove(struct sdio_func *func)
+{
+	sdio_claim_host(func);
+	sdio_disable_func(func);
+	sdio_release_host(func);
+	module_put(func->dev.driver->owner);
+}
+
+static int sdio_suspend(struct device *dev)
+{
+	int ret = 0;
+	/*
+	struct sdio_func *func = dev_to_sdio_func(dev);
+	ret = sdio_set_host_pm_flags(func, MMC_PM_KEEP_POWER);
+	if (ret)
+		sbus_printk(XRADIO_DBG_ERROR, "set MMC_PM_KEEP_POWER error\n");
+	*/
+	return ret;
+}
+
+static int sdio_resume(struct device *dev)
+{
+	return 0;
+}
+
+static const struct dev_pm_ops sdio_pm_ops = {
+	.suspend = sdio_suspend,
+	.resume  = sdio_resume,
+};
+
+static struct sdio_driver sdio_driver = {
+	.name     = "xradio_wlan",
+	.id_table = xradio_sdio_ids,
+	.probe    = sdio_probe,
+	.remove   = sdio_remove,
+	.drv = {
+			.owner = THIS_MODULE,
+			.pm = &sdio_pm_ops,
+	}
+};
+
+
+int xradio_sdio_register(){
+	return sdio_register_driver(&sdio_driver);
+}
+
+void xradio_sdio_unregister(){
+	sdio_unregister_driver(&sdio_driver);
+}
+
+MODULE_DEVICE_TABLE(sdio, xradio_sdio_ids);
diff --git a/drivers/net/wireless/xradio/sdio.h b/drivers/net/wireless/xradio/sdio.h
new file mode 100644
index 0000000..ea3c45a
--- /dev/null
+++ b/drivers/net/wireless/xradio/sdio.h
@@ -0,0 +1,17 @@
+#ifndef __XRADIO_SDIO_H
+#define __XRADIO_SDIO_H
+
+size_t sdio_align_len(struct xradio_common *self, size_t size);
+void sdio_lock(struct xradio_common *self);
+void sdio_unlock(struct xradio_common *self);
+int sdio_set_blk_size(struct xradio_common *self, size_t size);
+int sdio_data_read(struct xradio_common *self, unsigned int addr, void *dst,
+		int count);
+int sdio_data_write(struct xradio_common *self, unsigned int addr, const void *src,
+		int count);
+int sdio_pm(struct xradio_common *self, bool  suspend);
+
+int xradio_sdio_register(void);
+void xradio_sdio_unregister(void);
+
+#endif
diff --git a/drivers/net/wireless/xradio/sta.c b/drivers/net/wireless/xradio/sta.c
new file mode 100644
index 0000000..5e9d8a2
--- /dev/null
+++ b/drivers/net/wireless/xradio/sta.c
@@ -0,0 +1,2199 @@
+/*
+ * STA APIs for XRadio drivers
+ *
+ * Copyright (c) 2013, XRadio
+ * Author: XRadio
+ *
+ * This program is free software; you can 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/vmalloc.h>
+#include <linux/sched.h>
+#include <linux/firmware.h>
+#include <linux/if_arp.h>
+#include <linux/ipv6.h>
+#include <linux/icmpv6.h>
+#include <net/ndisc.h>
+
+#include "xradio.h"
+#include "sta.h"
+#include "ap.h"
+#include "keys.h"
+#include "fwio.h"
+#include "bh.h"
+#include "wsm.h"
+#ifdef ROAM_OFFLOAD
+#include <net/netlink.h>
+#endif /*ROAM_OFFLOAD*/
+
+#include "net/mac80211.h"
+
+#ifdef TES_P2P_0002_ROC_RESTART
+#include <linux/time.h>
+#endif
+
+#define WEP_ENCRYPT_HDR_SIZE    4
+#define WEP_ENCRYPT_TAIL_SIZE   4
+#define WPA_ENCRYPT_HDR_SIZE    8
+#define WPA_ENCRYPT_TAIL_SIZE   12
+#define WPA2_ENCRYPT_HDR_SIZE   8
+#define WPA2_ENCRYPT_TAIL_SIZE  8
+#define WAPI_ENCRYPT_HDR_SIZE   18
+#define WAPI_ENCRYPT_TAIL_SIZE  16
+#define MAX_ARP_REPLY_TEMPLATE_SIZE     120
+
+static inline void __xradio_free_event_queue(struct list_head *list)
+{
+	while (!list_empty(list)) {
+		struct xradio_wsm_event *event =
+			list_first_entry(list, struct xradio_wsm_event,link);
+		list_del(&event->link);
+		kfree(event);
+	}
+}
+
+static inline void __xradio_bf_configure(struct xradio_vif *priv)
+{
+	priv->bf_table.numOfIEs = __cpu_to_le32(3);
+	priv->bf_table.entry[0].ieId = WLAN_EID_VENDOR_SPECIFIC;
+	priv->bf_table.entry[0].actionFlags = 
+	                        WSM_BEACON_FILTER_IE_HAS_CHANGED       |
+	                        WSM_BEACON_FILTER_IE_NO_LONGER_PRESENT |
+	                        WSM_BEACON_FILTER_IE_HAS_APPEARED;
+
+	priv->bf_table.entry[0].oui[0] = 0x50;
+	priv->bf_table.entry[0].oui[1] = 0x6F;
+	priv->bf_table.entry[0].oui[2] = 0x9A;
+
+	priv->bf_table.entry[1].ieId = WLAN_EID_ERP_INFO;
+	priv->bf_table.entry[1].actionFlags = 
+	                        WSM_BEACON_FILTER_IE_HAS_CHANGED       |
+	                        WSM_BEACON_FILTER_IE_NO_LONGER_PRESENT |
+	                        WSM_BEACON_FILTER_IE_HAS_APPEARED;
+
+	priv->bf_table.entry[2].ieId = WLAN_EID_HT_OPERATION;
+	priv->bf_table.entry[2].actionFlags = 
+	                        WSM_BEACON_FILTER_IE_HAS_CHANGED       |
+	                        WSM_BEACON_FILTER_IE_NO_LONGER_PRESENT |
+	                        WSM_BEACON_FILTER_IE_HAS_APPEARED;
+
+	priv->bf_control.enabled = WSM_BEACON_FILTER_ENABLE;
+}
+
+/* ******************************************************************** */
+/* STA API								*/
+
+int xradio_start(struct ieee80211_hw *dev)
+{
+	struct xradio_common *hw_priv = dev->priv;
+	int ret = 0;
+
+
+	if (wait_event_interruptible_timeout(hw_priv->wsm_startup_done,
+				hw_priv->driver_ready, 3*HZ) <= 0) {
+		wiphy_err(dev->wiphy, "driver is not ready!\n");
+		return -ETIMEDOUT;
+	}
+
+	mutex_lock(&hw_priv->conf_mutex);
+
+	memcpy(hw_priv->mac_addr, dev->wiphy->perm_addr, ETH_ALEN);
+	hw_priv->softled_state = 0;
+
+	ret = xradio_setup_mac(hw_priv);
+	if (WARN_ON(ret)) {
+		wiphy_err(dev->wiphy, "xradio_setup_mac failed(%d)\n", ret);
+		goto out;
+	}
+
+out:
+	mutex_unlock(&hw_priv->conf_mutex);
+	return ret;
+}
+
+void xradio_stop(struct ieee80211_hw *dev)
+{
+	struct xradio_common *hw_priv = dev->priv;
+	struct xradio_vif *priv = NULL;
+	LIST_HEAD(list);
+	int i;
+
+	wsm_lock_tx(hw_priv);
+	while (down_trylock(&hw_priv->scan.lock)) {
+		/* Scan is in progress. Force it to stop. */
+		hw_priv->scan.req = NULL;
+		schedule();
+	}
+	up(&hw_priv->scan.lock);
+
+	cancel_delayed_work_sync(&hw_priv->scan.probe_work);
+	cancel_delayed_work_sync(&hw_priv->scan.timeout);
+	flush_workqueue(hw_priv->workqueue);
+	del_timer_sync(&hw_priv->ba_timer);
+
+	mutex_lock(&hw_priv->conf_mutex);
+
+	hw_priv->softled_state = 0;
+	/* xradio_set_leds(hw_priv); */
+
+	spin_lock(&hw_priv->event_queue_lock);
+	list_splice_init(&hw_priv->event_queue, &list);
+	spin_unlock(&hw_priv->event_queue_lock);
+	__xradio_free_event_queue(&list);
+
+	for (i = 0; i < 4; i++)
+		xradio_queue_clear(&hw_priv->tx_queue[i], XRWL_ALL_IFS);
+
+	/* HACK! */
+	if (atomic_xchg(&hw_priv->tx_lock, 1) != 1)
+		wiphy_debug(dev->wiphy, "TX is force-unlocked due to stop request.\n");
+
+	xradio_for_each_vif(hw_priv, priv, i) {
+		if (!priv)
+			continue;
+		priv->mode = NL80211_IFTYPE_UNSPECIFIED;
+		priv->listening = false;
+		priv->delayed_link_loss = 0;
+		priv->join_status = XRADIO_JOIN_STATUS_PASSIVE;
+		cancel_delayed_work_sync(&priv->join_timeout);
+		cancel_delayed_work_sync(&priv->bss_loss_work);
+		cancel_delayed_work_sync(&priv->connection_loss_work);
+		cancel_delayed_work_sync(&priv->link_id_gc_work);
+		del_timer_sync(&priv->mcast_timeout);
+	}
+
+	wsm_unlock_tx(hw_priv);
+
+	mutex_unlock(&hw_priv->conf_mutex);
+}
+
+int xradio_add_interface(struct ieee80211_hw *dev,
+			 struct ieee80211_vif *vif)
+{
+	int ret;
+	struct xradio_common *hw_priv = dev->priv;
+	struct xradio_vif *priv;
+	struct xradio_vif **drv_priv = (void *)vif->drv_priv;
+	int i;
+	if (atomic_read(&hw_priv->num_vifs) >= XRWL_MAX_VIFS)
+		return -EOPNOTSUPP;
+
+	if (wait_event_interruptible_timeout(hw_priv->wsm_startup_done,
+				hw_priv->driver_ready, 3*HZ) <= 0) {
+		wiphy_err(dev->wiphy, "driver is not ready!\n");
+		return -ETIMEDOUT;
+	}
+
+	/* fix the problem that when connected,then deauth */
+	vif->driver_flags |= IEEE80211_VIF_BEACON_FILTER;
+	vif->driver_flags |= IEEE80211_VIF_SUPPORTS_UAPSD;
+
+	priv = xrwl_get_vif_from_ieee80211(vif);
+	atomic_set(&priv->enabled, 0);
+
+	*drv_priv = priv;
+	/* __le32 auto_calibration_mode = __cpu_to_le32(1); */
+
+	mutex_lock(&hw_priv->conf_mutex);
+
+	priv->mode = vif->type;
+
+	spin_lock(&hw_priv->vif_list_lock);
+	if (atomic_read(&hw_priv->num_vifs) < XRWL_MAX_VIFS) {
+		for (i = 0; i < XRWL_MAX_VIFS; i++)
+			if (!memcmp(vif->addr, hw_priv->addresses[i].addr, ETH_ALEN))
+				break;
+		if (i == XRWL_MAX_VIFS) {
+			spin_unlock(&hw_priv->vif_list_lock);
+			mutex_unlock(&hw_priv->conf_mutex);
+			return -EINVAL;
+		}
+		priv->if_id = i;
+
+		hw_priv->if_id_slot |= BIT(priv->if_id);
+		priv->hw_priv = hw_priv;
+		priv->hw      = dev;
+		priv->vif     = vif;
+		hw_priv->vif_list[priv->if_id] = vif;
+		atomic_inc(&hw_priv->num_vifs);
+	} else {
+		spin_unlock(&hw_priv->vif_list_lock);
+		mutex_unlock(&hw_priv->conf_mutex);
+		return -EOPNOTSUPP;
+	}
+	spin_unlock(&hw_priv->vif_list_lock);
+	/* TODO:COMBO :Check if MAC address matches the one expected by FW */
+	memcpy(hw_priv->mac_addr, vif->addr, ETH_ALEN);
+
+	/* Enable auto-calibration */
+	/* Exception in subsequent channel switch; disabled.
+	WARN_ON(wsm_write_mib(hw_priv, WSM_MIB_ID_SET_AUTO_CALIBRATION_MODE,
+		&auto_calibration_mode, sizeof(auto_calibration_mode)));
+	*/
+	wiphy_debug(dev->wiphy, "Interface ID:%d of type:%d added\n", priv->if_id, priv->mode);
+	mutex_unlock(&hw_priv->conf_mutex);
+
+	xradio_vif_setup(priv);
+
+	ret = WARN_ON(xradio_setup_mac_pvif(priv));
+
+	return ret;
+}
+
+void xradio_remove_interface(struct ieee80211_hw *dev,
+			     struct ieee80211_vif *vif)
+{
+	struct xradio_common *hw_priv = dev->priv;
+	struct xradio_vif *priv = xrwl_get_vif_from_ieee80211(vif);
+	struct wsm_reset reset = {
+		.reset_statistics = true,
+	};
+	int i;
+	bool is_htcapie = false;
+	struct xradio_vif *tmp_priv;
+
+	wiphy_warn(dev->wiphy, "!!! vif_id=%d\n", priv->if_id);
+	atomic_set(&priv->enabled, 0);
+	down(&hw_priv->scan.lock);
+	if(priv->join_status == XRADIO_JOIN_STATUS_STA){
+		if (atomic_xchg(&priv->delayed_unjoin, 0)) {
+			wsm_unlock_tx(hw_priv);
+			wiphy_err(dev->wiphy, "delayed_unjoin exist!\n");
+		}
+		cancel_work_sync(&priv->unjoin_work);
+		wsm_lock_tx(hw_priv);
+		xradio_unjoin_work(&priv->unjoin_work);
+	}
+	mutex_lock(&hw_priv->conf_mutex);
+	xradio_tx_queues_lock(hw_priv);
+	wsm_lock_tx(hw_priv);
+	switch (priv->join_status) {
+	case XRADIO_JOIN_STATUS_AP:
+		for (i = 0; priv->link_id_map; ++i) {
+			if (priv->link_id_map & BIT(i)) {
+				xrwl_unmap_link(priv, i);
+				priv->link_id_map &= ~BIT(i);
+			}
+		}
+		memset(priv->link_id_db, 0,
+				sizeof(priv->link_id_db));
+		priv->sta_asleep_mask = 0;
+		priv->enable_beacon = false;
+		priv->tx_multicast = false;
+		priv->aid0_bit_set = false;
+		priv->buffered_multicasts = false;
+		priv->pspoll_mask = 0;
+		reset.link_id = 0;
+		wsm_reset(hw_priv, &reset, priv->if_id);
+		WARN_ON(wsm_set_operational_mode(hw_priv, &defaultoperationalmode, priv->if_id));
+		xradio_for_each_vif(hw_priv, tmp_priv, i) {
+			if (!tmp_priv)
+				continue;
+			if ((tmp_priv->join_status == XRADIO_JOIN_STATUS_STA) && tmp_priv->htcap)
+				is_htcapie = true;
+		}
+
+		if (is_htcapie) {
+			hw_priv->vif0_throttle = XRWL_HOST_VIF0_11N_THROTTLE;
+			hw_priv->vif1_throttle = XRWL_HOST_VIF1_11N_THROTTLE;
+			sta_printk(XRADIO_DBG_NIY, "AP REMOVE HTCAP 11N %d\n",hw_priv->vif0_throttle);
+		} else {
+			hw_priv->vif0_throttle = XRWL_HOST_VIF0_11BG_THROTTLE;
+			hw_priv->vif1_throttle = XRWL_HOST_VIF1_11BG_THROTTLE;
+			sta_printk(XRADIO_DBG_NIY, "AP REMOVE 11BG %d\n",hw_priv->vif0_throttle);
+		}
+		break;
+	case XRADIO_JOIN_STATUS_MONITOR:
+		xradio_disable_listening(priv);
+		break;
+	default:
+		break;
+	}
+	/* TODO:COMBO: Change Queue Module */
+	__xradio_flush(hw_priv, false, priv->if_id);
+
+	cancel_delayed_work_sync(&priv->bss_loss_work);
+	cancel_delayed_work_sync(&priv->connection_loss_work);
+	cancel_delayed_work_sync(&priv->link_id_gc_work);
+	cancel_delayed_work_sync(&priv->join_timeout);
+	cancel_delayed_work_sync(&priv->set_cts_work);
+	cancel_delayed_work_sync(&priv->pending_offchanneltx_work);
+
+	del_timer_sync(&priv->mcast_timeout);
+	/* TODO:COMBO: May be reset of these variables "delayed_link_loss and
+	 * join_status to default can be removed as dev_priv will be freed by
+	 * mac80211 */
+	priv->delayed_link_loss = 0;
+	priv->join_status = XRADIO_JOIN_STATUS_PASSIVE;
+	wsm_unlock_tx(hw_priv);
+
+	if ((priv->if_id ==1) && (priv->mode == NL80211_IFTYPE_AP
+		|| priv->mode == NL80211_IFTYPE_P2P_GO)) {
+		hw_priv->is_go_thru_go_neg = false;
+	}
+	spin_lock(&hw_priv->vif_list_lock);
+	spin_lock(&priv->vif_lock);
+	hw_priv->vif_list[priv->if_id] = NULL;
+	hw_priv->if_id_slot &= (~BIT(priv->if_id));
+	atomic_dec(&hw_priv->num_vifs);
+	if (atomic_read(&hw_priv->num_vifs) == 0) {
+		xradio_free_keys(hw_priv);
+		memset(hw_priv->mac_addr, 0, ETH_ALEN);
+	}
+	spin_unlock(&priv->vif_lock);
+	spin_unlock(&hw_priv->vif_list_lock);
+	priv->listening = false;
+
+	xradio_tx_queues_unlock(hw_priv);
+	mutex_unlock(&hw_priv->conf_mutex);
+
+	if (atomic_read(&hw_priv->num_vifs) == 0)
+		flush_workqueue(hw_priv->workqueue);
+	memset(priv, 0, sizeof(struct xradio_vif));
+	up(&hw_priv->scan.lock);
+}
+
+int xradio_change_interface(struct ieee80211_hw *dev,
+				struct ieee80211_vif *vif,
+				enum nl80211_iftype new_type,
+				bool p2p)
+{
+	int ret = 0;
+	wiphy_debug(dev->wiphy, "changing interface type; new type=%d(%d), p2p=%d(%d)\n",
+			new_type, vif->type, p2p, vif->p2p);
+	if (new_type != vif->type || vif->p2p != p2p) {
+		xradio_remove_interface(dev, vif);
+		vif->type = new_type;
+		vif->p2p = p2p;
+		ret = xradio_add_interface(dev, vif);
+	}
+
+	return ret;
+}
+
+int xradio_config(struct ieee80211_hw *dev, u32 changed)
+{
+	int ret = 0;
+	struct xradio_common *hw_priv = dev->priv;
+	struct ieee80211_conf *conf = &dev->conf;
+	/* TODO:COMBO: adjust to multi vif interface
+	 * IEEE80211_CONF_CHANGE_IDLE is still handled per xradio_vif*/
+	int if_id = 0;
+	struct xradio_vif *priv;
+
+
+	if (changed &
+		(IEEE80211_CONF_CHANGE_MONITOR|IEEE80211_CONF_CHANGE_IDLE)) {
+		/* TBD: It looks like it's transparent
+		 * there's a monitor interface present -- use this
+		 * to determine for example whether to calculate
+		 * timestamps for packets or not, do not use instead
+		 * of filter flags! */
+		wiphy_debug(dev->wiphy, "ignore IEEE80211_CONF_CHANGE_MONITOR (%d)"
+		           "IEEE80211_CONF_CHANGE_IDLE (%d)\n",
+		           (changed & IEEE80211_CONF_CHANGE_MONITOR) ? 1 : 0,
+		           (changed & IEEE80211_CONF_CHANGE_IDLE) ? 1 : 0);
+		return ret;
+	}
+
+	down(&hw_priv->scan.lock);
+	mutex_lock(&hw_priv->conf_mutex);
+	priv = __xrwl_hwpriv_to_vifpriv(hw_priv, hw_priv->scan.if_id);
+	/* TODO: IEEE80211_CONF_CHANGE_QOS */
+	/* TODO:COMBO:Change when support is available mac80211*/
+	if (changed & IEEE80211_CONF_CHANGE_POWER) {
+		/*hw_priv->output_power = conf->power_level;*/
+		hw_priv->output_power = 20;
+		wiphy_debug(dev->wiphy, "Config Tx power=%d, but real=%d\n",
+		           conf->power_level, hw_priv->output_power);
+		WARN_ON(wsm_set_output_power(hw_priv, hw_priv->output_power * 10, if_id));
+	}
+
+	if ((changed & IEEE80211_CONF_CHANGE_CHANNEL) &&
+	    (hw_priv->channel != conf->chandef.chan)) {
+		/* Switch Channel commented for CC Mode */
+		struct ieee80211_channel *ch = conf->chandef.chan;
+		wiphy_debug(dev->wiphy, "Freq %d (wsm ch: %d).\n",
+		           ch->center_freq, ch->hw_value);
+		/* Earlier there was a call to __xradio_flush().
+		   Removed as deemed unnecessary */
+			hw_priv->channel = ch;
+			hw_priv->channel_changed = 1;
+	}
+
+	mutex_unlock(&hw_priv->conf_mutex);
+	up(&hw_priv->scan.lock);
+	return ret;
+}
+
+void xradio_update_filtering(struct xradio_vif *priv)
+{
+	int ret;
+	bool bssid_filtering = !priv->rx_filter.bssid;
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	static struct wsm_beacon_filter_control bf_disabled = {
+		.enabled = 0,
+		.bcn_count = 1,
+	};
+	bool ap_mode = 0;
+	static struct wsm_beacon_filter_table bf_table_auto = {
+		.numOfIEs = __cpu_to_le32(2),
+		.entry[0].ieId = WLAN_EID_VENDOR_SPECIFIC,
+		.entry[0].actionFlags = WSM_BEACON_FILTER_IE_HAS_CHANGED |
+					WSM_BEACON_FILTER_IE_NO_LONGER_PRESENT |
+					WSM_BEACON_FILTER_IE_HAS_APPEARED,
+		.entry[0].oui[0] = 0x50,
+		.entry[0].oui[1] = 0x6F,
+		.entry[0].oui[2] = 0x9A,
+
+		.entry[1].ieId = WLAN_EID_HT_OPERATION,
+		.entry[1].actionFlags = WSM_BEACON_FILTER_IE_HAS_CHANGED |
+					WSM_BEACON_FILTER_IE_NO_LONGER_PRESENT |
+					WSM_BEACON_FILTER_IE_HAS_APPEARED,
+	};
+	static struct wsm_beacon_filter_control bf_auto = {
+		.enabled = WSM_BEACON_FILTER_ENABLE |
+			WSM_BEACON_FILTER_AUTO_ERP,
+		.bcn_count = 1,
+	};
+
+
+	bf_auto.bcn_count = priv->bf_control.bcn_count;
+
+	if (priv->join_status == XRADIO_JOIN_STATUS_PASSIVE)
+		return;
+	else if (priv->join_status == XRADIO_JOIN_STATUS_MONITOR)
+		bssid_filtering = false;
+
+	if (priv->vif && (priv->vif->type == NL80211_IFTYPE_AP))
+		ap_mode = true;
+	/*
+	* When acting as p2p client being connected to p2p GO, in order to
+	* receive frames from a different p2p device, turn off bssid filter.
+	*
+	* WARNING: FW dependency!
+	* This can only be used with FW WSM371 and its successors.
+	* In that FW version even with bssid filter turned off,
+	* device will block most of the unwanted frames.
+	*/
+	if (priv->vif && priv->vif->p2p)
+		bssid_filtering = false;
+
+	ret = wsm_set_rx_filter(hw_priv, &priv->rx_filter, priv->if_id);
+	if (!ret && !ap_mode) {
+		if (priv->vif) {
+			if (priv->vif->p2p || NL80211_IFTYPE_STATION != priv->vif->type)
+				ret = wsm_set_beacon_filter_table(hw_priv, &priv->bf_table, priv->if_id);
+			else
+				ret = wsm_set_beacon_filter_table(hw_priv, &bf_table_auto, priv->if_id);
+		} else
+			WARN_ON(1);
+	}
+	if (!ret && !ap_mode) {
+		if (priv->disable_beacon_filter)
+			ret = wsm_beacon_filter_control(hw_priv, &bf_disabled, priv->if_id);
+		else {
+			if (priv->vif) {
+				if (priv->vif->p2p || NL80211_IFTYPE_STATION != priv->vif->type)
+					ret = wsm_beacon_filter_control(hw_priv, &priv->bf_control,
+					                                 priv->if_id);
+				else
+					ret = wsm_beacon_filter_control(hw_priv, &bf_auto, priv->if_id);
+			} else
+				WARN_ON(1);
+		}
+	}
+
+	if (!ret)
+		ret = wsm_set_bssid_filtering(hw_priv, bssid_filtering, priv->if_id);
+#if 0
+	if (!ret) {
+		ret = wsm_set_multicast_filter(hw_priv, &priv->multicast_filter, priv->if_id);
+	}
+#endif
+	if (ret)
+		wiphy_debug(priv->hw_priv->hw->wiphy, "Update filtering failed: %d.\n", ret);
+	return;
+}
+
+void xradio_update_filtering_work(struct work_struct *work)
+{
+	struct xradio_vif *priv =
+		container_of(work, struct xradio_vif,
+		update_filtering_work);
+
+	xradio_update_filtering(priv);
+}
+
+void xradio_set_beacon_wakeup_period_work(struct work_struct *work)
+{
+	
+	struct xradio_vif *priv = 
+	       container_of(work, struct xradio_vif, set_beacon_wakeup_period_work);
+
+
+#ifdef XRADIO_USE_LONG_DTIM_PERIOD
+{
+	int join_dtim_period_extend;
+	if (priv->join_dtim_period <= 3) {
+		join_dtim_period_extend = priv->join_dtim_period * 3;
+	} else if (priv->join_dtim_period <= 5) {
+		join_dtim_period_extend = priv->join_dtim_period * 2;
+	} else {
+		join_dtim_period_extend = priv->join_dtim_period;
+	}
+	WARN_ON(wsm_set_beacon_wakeup_period(priv->hw_priv,
+	         priv->beacon_int * join_dtim_period_extend >
+	         MAX_BEACON_SKIP_TIME_MS ? 1 : join_dtim_period_extend, 
+	         0, priv->if_id));
+}
+#else
+	WARN_ON(wsm_set_beacon_wakeup_period(priv->hw_priv,
+	         priv->beacon_int * priv->join_dtim_period >
+	         MAX_BEACON_SKIP_TIME_MS ? 1 :priv->join_dtim_period, 
+	         0, priv->if_id));
+#endif
+}
+
+u64 xradio_prepare_multicast(struct ieee80211_hw *hw,
+							struct netdev_hw_addr_list *mc_list)
+{
+	struct xradio_common *hw_priv = hw->priv;
+	struct xradio_vif *priv = NULL;
+	static u8 broadcast_ipv6[ETH_ALEN] = {
+		0x33, 0x33, 0x00, 0x00, 0x00, 0x01
+	};
+	static u8 broadcast_ipv4[ETH_ALEN] = {
+		0x01, 0x00, 0x5e, 0x00, 0x00, 0x01
+	};
+	
+	int i= 0;
+	xradio_for_each_vif(hw_priv,priv,i) {
+		struct netdev_hw_addr *ha = NULL;
+		int count = 0;
+		if ((!priv))
+			continue;
+
+		/* Disable multicast filtering */
+		priv->has_multicast_subscription = false;
+		memset(&priv->multicast_filter, 0x00, sizeof(priv->multicast_filter));
+		if (netdev_hw_addr_list_count(mc_list) > WSM_MAX_GRP_ADDRTABLE_ENTRIES)
+			return 0;
+
+		/* Enable if requested */
+		netdev_hw_addr_list_for_each(ha, mc_list) {
+			sta_printk(XRADIO_DBG_MSG, "multicast: %pM\n", ha->addr);
+			memcpy(&priv->multicast_filter.macAddress[count], ha->addr, ETH_ALEN);
+			if (memcmp(ha->addr, broadcast_ipv4, ETH_ALEN) &&
+		    	memcmp(ha->addr, broadcast_ipv6, ETH_ALEN))
+				priv->has_multicast_subscription = true;
+			count++;
+		}
+		if (count) {
+			priv->multicast_filter.enable = __cpu_to_le32(1);
+			priv->multicast_filter.numOfAddresses = __cpu_to_le32(count);
+		}
+	}
+	return netdev_hw_addr_list_count(mc_list);
+}
+
+void xradio_configure_filter(struct ieee80211_hw *hw,
+                             unsigned int changed_flags,
+                             unsigned int *total_flags,
+                             u64 multicast)
+{
+	struct xradio_common *hw_priv = hw->priv;
+	struct xradio_vif *priv = NULL;
+	int i = 0;
+
+	/* delete umac warning */
+	if (hw_priv->vif_list[0] == NULL && hw_priv->vif_list[1] == NULL)
+
+		*total_flags &= ~(1<<31);
+		
+	xradio_for_each_vif(hw_priv, priv, i) {
+		if(NULL == priv)
+			continue;
+
+#if 0
+		bool listening = !!(*total_flags &
+	                     	(FIF_PROMISC_IN_BSS      |
+	                      	FIF_OTHER_BSS           |
+	                      	FIF_BCN_PRBRESP_PROMISC |
+	                      	FIF_PROBE_REQ));
+#endif
+
+		*total_flags &= FIF_OTHER_BSS      |
+	                	FIF_FCSFAIL        |
+	                	FIF_BCN_PRBRESP_PROMISC |
+	                	FIF_PROBE_REQ;
+
+		down(&hw_priv->scan.lock);
+		mutex_lock(&hw_priv->conf_mutex);
+
+		priv->rx_filter.promiscuous = 0;
+		priv->rx_filter.bssid = (*total_flags & 
+	                         	(FIF_OTHER_BSS | FIF_PROBE_REQ)) ? 1 : 0;
+		priv->rx_filter.fcs = (*total_flags & FIF_FCSFAIL) ? 1 : 0;
+		priv->bf_control.bcn_count = (*total_flags &
+	                              	(FIF_BCN_PRBRESP_PROMISC |
+	                               	FIF_PROBE_REQ)) ? 1 : 0;
+
+		/*add for handle ap FIF_PROBE_REQ message,*/
+		priv->rx_filter.promiscuous = 0;
+		priv->rx_filter.fcs = 0;
+		if(NL80211_IFTYPE_AP == priv->vif->type){
+			priv->bf_control.bcn_count = 1;
+			priv->rx_filter.bssid = 1; 	
+		}else{
+			priv->bf_control.bcn_count = 0;
+			priv->rx_filter.bssid = 0; 
+		}
+#if 0
+		if (priv->listening ^ listening) {
+			priv->listening = listening;
+			wsm_lock_tx(hw_priv);
+			xradio_update_listening(priv, listening);
+			wsm_unlock_tx(hw_priv);
+		}
+#endif
+		xradio_update_filtering(priv);
+		mutex_unlock(&hw_priv->conf_mutex);
+		up(&hw_priv->scan.lock);
+	}
+}
+
+int xradio_conf_tx(struct ieee80211_hw *dev, struct ieee80211_vif *vif,
+                   u16 queue, const struct ieee80211_tx_queue_params *params)
+{
+	struct xradio_common *hw_priv = dev->priv;
+	struct xradio_vif *priv = xrwl_get_vif_from_ieee80211(vif);
+	int ret = 0;
+	/* To prevent re-applying PM request OID again and again*/
+	bool old_uapsdFlags;
+
+	wiphy_debug(dev->wiphy, "vif %d, configuring tx\n", priv->if_id);
+
+	if (WARN_ON(!priv))
+		return -EOPNOTSUPP;
+
+	mutex_lock(&hw_priv->conf_mutex);
+
+	if (queue < dev->queues) {
+		old_uapsdFlags = priv->uapsd_info.uapsdFlags;
+
+		WSM_TX_QUEUE_SET(&priv->tx_queue_params, queue, 0, 0, 0);
+		ret = wsm_set_tx_queue_params(hw_priv,
+		                              &priv->tx_queue_params.params[queue],
+		                              queue, priv->if_id);
+		if (ret) {
+			wiphy_err(dev->wiphy, "wsm_set_tx_queue_params failed!\n");
+			ret = -EINVAL;
+			goto out;
+		}
+
+		WSM_EDCA_SET(&priv->edca, queue, params->aifs, 
+		              params->cw_min, params->cw_max, 
+		              params->txop, 0xc8, params->uapsd);
+		/* sta role is not support  the uapsd */ 
+		if (priv->mode == NL80211_IFTYPE_STATION || 
+				priv->mode == NL80211_IFTYPE_P2P_CLIENT)
+			priv->edca.params[queue].uapsdEnable = 0;
+
+		ret = wsm_set_edca_params(hw_priv, &priv->edca, priv->if_id);
+		if (ret) {
+			wiphy_err(dev->wiphy, "wsm_set_edca_params failed!\n");
+			ret = -EINVAL;
+			goto out;
+		}
+
+		if (priv->mode == NL80211_IFTYPE_STATION) {
+			ret = xradio_set_uapsd_param(priv, &priv->edca);
+			if (!ret && priv->setbssparams_done &&
+			    (priv->join_status == XRADIO_JOIN_STATUS_STA) &&
+			    (old_uapsdFlags != priv->uapsd_info.uapsdFlags))
+				xradio_set_pm(priv, &priv->powersave_mode);
+		}
+	} else {
+		wiphy_err(dev->wiphy, "queue is to large!\n");
+		ret = -EINVAL;
+	}
+
+out:
+	mutex_unlock(&hw_priv->conf_mutex);
+	return ret;
+}
+
+int xradio_get_stats(struct ieee80211_hw *dev,
+		     struct ieee80211_low_level_stats *stats)
+{
+	struct xradio_common *hw_priv = dev->priv;
+
+	memcpy(stats, &hw_priv->stats, sizeof(*stats));
+	return 0;
+}
+
+/*
+int xradio_get_tx_stats(struct ieee80211_hw *dev,
+			struct ieee80211_tx_queue_stats *stats)
+{
+	int i;
+	struct xradio_common *priv = dev->priv;
+
+	for (i = 0; i < dev->queues; ++i)
+		xradio_queue_get_stats(&priv->tx_queue[i], &stats[i]);
+
+	return 0;
+}
+*/
+
+int xradio_set_pm(struct xradio_vif *priv, const struct wsm_set_pm *arg)
+{
+	struct wsm_set_pm pm = *arg;
+
+	if (priv->uapsd_info.uapsdFlags != 0)
+		pm.pmMode &= ~WSM_PSM_FAST_PS_FLAG;
+
+	if (memcmp(&pm, &priv->firmware_ps_mode, sizeof(struct wsm_set_pm))) {
+		priv->firmware_ps_mode = pm;
+		return wsm_set_pm(priv->hw_priv, &pm, priv->if_id);
+	} else {
+		return 0;
+	}
+}
+
+void xradio_wep_key_work(struct work_struct *work)
+{
+	struct xradio_vif *priv = container_of(work, struct xradio_vif , wep_key_work);
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	u8 queueId = xradio_queue_get_queue_id(hw_priv->pending_frame_id);
+	struct xradio_queue *queue = &hw_priv->tx_queue[queueId];
+	__le32 wep_default_key_id = __cpu_to_le32(priv->wep_default_key_id);
+
+
+	BUG_ON(queueId >= 4);
+
+	sta_printk(XRADIO_DBG_MSG, "Setting default WEP key: %d\n", 
+	           priv->wep_default_key_id);
+
+	wsm_flush_tx(hw_priv);
+	WARN_ON(wsm_write_mib(hw_priv, WSM_MIB_ID_DOT11_WEP_DEFAULT_KEY_ID,
+	                       &wep_default_key_id, sizeof(wep_default_key_id),
+	                       priv->if_id));
+
+	xradio_queue_requeue(queue, hw_priv->pending_frame_id, true);
+
+	wsm_unlock_tx(hw_priv);
+}
+
+int xradio_set_rts_threshold(struct ieee80211_hw *hw, u32 value)
+{
+	struct xradio_common *hw_priv = hw->priv;
+	int ret = 0;
+	__le32 val32;
+	struct xradio_vif *priv = NULL;
+	int i =0;
+	int if_id;
+
+
+	xradio_for_each_vif(hw_priv,priv,i) {
+		if (!priv)
+			continue;
+		if_id = priv->if_id;
+
+		if (value != (u32) -1)
+			val32 = __cpu_to_le32(value);
+		else
+			val32 = 0; /* disabled */
+
+		/* mutex_lock(&priv->conf_mutex); */
+		ret = WARN_ON(wsm_write_mib(hw_priv, WSM_MIB_ID_DOT11_RTS_THRESHOLD,
+			&val32, sizeof(val32), if_id));
+		/* mutex_unlock(&priv->conf_mutex); */
+	}
+	return ret;
+}
+
+/* TODO: COMBO: Flush only a particular interface specific parts */
+int __xradio_flush(struct xradio_common *hw_priv, bool drop, int if_id)
+{
+	int i, ret;
+	struct xradio_vif *priv =
+		__xrwl_hwpriv_to_vifpriv(hw_priv, if_id);
+
+
+	for (;;) {
+		/* TODO: correct flush handling is required when dev_stop.
+		 * Temporary workaround: 2s
+		 */
+		if (drop) {
+			for (i = 0; i < 4; ++i)
+				xradio_queue_clear(&hw_priv->tx_queue[i],if_id);
+		} else if(!hw_priv->bh_error){
+			ret = wait_event_timeout(
+				hw_priv->tx_queue_stats.wait_link_id_empty,
+				xradio_queue_stats_is_empty(&hw_priv->tx_queue_stats, -1, if_id),
+				2 * HZ);
+		} else { //add by yangfh, don't wait when bh error
+			sta_printk(XRADIO_DBG_ERROR, " %s:bh_error occur.\n", __func__);
+			ret = -1;
+			break;
+		}
+
+		if (!drop && unlikely(ret <= 0)) {
+			sta_printk(XRADIO_DBG_ERROR, " %s: timeout...\n", __func__);
+			ret = -ETIMEDOUT;
+			break;
+		} else {
+			ret = 0;
+		}
+
+		wsm_vif_lock_tx(priv);
+		if (unlikely(!xradio_queue_stats_is_empty(&hw_priv->tx_queue_stats,
+			          -1, if_id))) {
+			/* Highly unlekely: WSM requeued frames. */
+			wsm_unlock_tx(hw_priv);
+			continue;
+		}
+		wsm_unlock_tx(hw_priv);
+		break;
+	}
+	return ret;
+}
+
+void xradio_flush(struct ieee80211_hw *hw, struct ieee80211_vif *vif, u32 queues, bool drop)
+{
+	//struct xradio_vif *priv = NULL;
+	struct xradio_common *hw_priv = hw->priv;
+	int i = 0;
+	struct xradio_vif *priv = xrwl_get_vif_from_ieee80211(vif);
+
+	/*TODO:COMBO: reenable this part of code when flush callback
+	 * is implemented per vif */
+	/*switch (hw_priv->mode) {
+	case NL80211_IFTYPE_MONITOR:
+		drop = true;
+		break;
+	case NL80211_IFTYPE_AP:
+		if (!hw_priv->enable_beacon)
+			drop = true;
+		break;
+	}*/
+
+	//if (!(hw_priv->if_id_slot & BIT(priv->if_id)))
+	//	return;
+
+	xradio_for_each_vif(hw_priv, priv, i) {
+		if(NULL == priv)
+			continue;
+		if ((hw_priv->if_id_slot & BIT(priv->if_id)))
+			__xradio_flush(hw_priv, drop, priv->if_id);
+	}
+	return;
+}
+
+int xradio_remain_on_channel(struct ieee80211_hw *hw,
+			     struct ieee80211_vif *vif,
+			     struct ieee80211_channel *chan,
+			     int duration, enum ieee80211_roc_type type)
+{
+	int ret = 0;
+	struct xradio_common *hw_priv = hw->priv;
+	struct xradio_vif *priv = NULL;
+	int i = 0;
+	int if_id;
+#ifdef	TES_P2P_0002_ROC_RESTART
+	struct timeval TES_P2P_0002_tmval;
+#endif
+
+
+#ifdef	TES_P2P_0002_ROC_RESTART
+	do_gettimeofday(&TES_P2P_0002_tmval);
+	TES_P2P_0002_roc_dur  = (s32)duration;
+	TES_P2P_0002_roc_sec  = (s32)TES_P2P_0002_tmval.tv_sec;
+	TES_P2P_0002_roc_usec = (s32)TES_P2P_0002_tmval.tv_usec;
+#endif
+
+	down(&hw_priv->scan.lock);
+	mutex_lock(&hw_priv->conf_mutex);
+	xradio_for_each_vif(hw_priv, priv, i) {
+		if(NULL == priv)
+			continue;
+		if_id = priv->if_id;
+
+#ifdef ROC_DEBUG
+		sta_printk(XRADIO_DBG_WARN, "ROC IN %d ch %d\n", 
+		           priv->if_id, chan->hw_value);
+#endif
+		/* default only p2p interface if_id can remain on */
+		if((priv->if_id == 0) || (priv->if_id == 1))
+			continue;
+		hw_priv->roc_if_id = priv->if_id;
+		ret = WARN_ON(__xradio_flush(hw_priv, false, if_id));
+		xradio_enable_listening(priv, chan);
+
+		if (!ret) {
+			atomic_set(&hw_priv->remain_on_channel, 1);
+			queue_delayed_work(hw_priv->workqueue, &hw_priv->rem_chan_timeout,
+			                   duration * HZ / 1000);
+			priv->join_status = XRADIO_JOIN_STATUS_MONITOR;
+			ieee80211_ready_on_channel(hw);
+		} else {
+			hw_priv->roc_if_id = -1;
+			up(&hw_priv->scan.lock);
+		}
+
+#ifdef ROC_DEBUG
+		sta_printk(XRADIO_DBG_WARN, "ROC OUT %d\n", priv->if_id);
+#endif
+			}
+		/* set the channel to supplied ieee80211_channel pointer, if it
+	        is not set. This is to remove the crash while sending a probe res
+	        in listen state. Later channel will updated on
+	        IEEE80211_CONF_CHANGE_CHANNEL event*/
+		if(!hw_priv->channel) {
+			hw_priv->channel = chan;
+		}
+		mutex_unlock(&hw_priv->conf_mutex);
+	return ret;
+}
+
+int xradio_cancel_remain_on_channel(struct ieee80211_hw *hw)
+{
+	struct xradio_common *hw_priv = hw->priv;
+
+
+	sta_printk(XRADIO_DBG_NIY, "Cancel remain on channel\n");
+#ifdef TES_P2P_0002_ROC_RESTART
+	if (TES_P2P_0002_state == TES_P2P_0002_STATE_GET_PKTID) {
+		TES_P2P_0002_state = TES_P2P_0002_STATE_IDLE;
+		sta_printk(XRADIO_DBG_WARN, "[ROC_RESTART_STATE_IDLE][Cancel ROC]\n");
+	}
+#endif
+
+	if (atomic_read(&hw_priv->remain_on_channel))
+		cancel_delayed_work_sync(&hw_priv->rem_chan_timeout);
+
+	if (atomic_read(&hw_priv->remain_on_channel))
+		xradio_rem_chan_timeout(&hw_priv->rem_chan_timeout.work);
+
+	return 0;
+}
+
+/* ******************************************************************** */
+/* WSM callbacks							*/
+
+void xradio_channel_switch_cb(struct xradio_common *hw_priv)
+{
+	wsm_unlock_tx(hw_priv);
+}
+
+void xradio_free_event_queue(struct xradio_common *hw_priv)
+{
+	LIST_HEAD(list);
+
+
+	spin_lock(&hw_priv->event_queue_lock);
+	list_splice_init(&hw_priv->event_queue, &list);
+	spin_unlock(&hw_priv->event_queue_lock);
+
+	__xradio_free_event_queue(&list);
+}
+
+void xradio_event_handler(struct work_struct *work)
+{
+	struct xradio_common *hw_priv =
+		container_of(work, struct xradio_common, event_handler);
+	struct xradio_vif *priv;
+	struct xradio_wsm_event *event;
+	LIST_HEAD(list);
+
+
+	spin_lock(&hw_priv->event_queue_lock);
+	list_splice_init(&hw_priv->event_queue, &list);
+	spin_unlock(&hw_priv->event_queue_lock);
+
+	mutex_lock(&hw_priv->conf_mutex);
+	list_for_each_entry(event, &list, link) {
+		priv = __xrwl_hwpriv_to_vifpriv(hw_priv, event->if_id);
+		if (!priv) {
+			sta_printk(XRADIO_DBG_WARN, "[CQM] Event for non existing "
+			           "interface, ignoring.\n");
+			continue;
+		}
+		switch (event->evt.eventId) {
+			case WSM_EVENT_ERROR:
+				/* I even don't know what is it about.. */
+				//STUB();
+				break;
+			case WSM_EVENT_BSS_LOST:
+			{
+				spin_lock(&priv->bss_loss_lock);
+				if (priv->bss_loss_status > XRADIO_BSS_LOSS_NONE) {
+					spin_unlock(&priv->bss_loss_lock);
+					break;
+				}
+				priv->bss_loss_status = XRADIO_BSS_LOSS_CHECKING;
+				spin_unlock(&priv->bss_loss_lock);
+				sta_printk(XRADIO_DBG_WARN, "[CQM] BSS lost, Beacon miss=%d, event=%x.\n",
+				           (event->evt.eventData>>8)&0xff, event->evt.eventData&0xff);
+
+				cancel_delayed_work_sync(&priv->bss_loss_work);
+				cancel_delayed_work_sync(&priv->connection_loss_work);
+				if (!down_trylock(&hw_priv->scan.lock)) {
+					up(&hw_priv->scan.lock);
+					priv->delayed_link_loss = 0;
+					queue_delayed_work(hw_priv->workqueue,
+							&priv->bss_loss_work, HZ/10); //100ms
+				} else {
+					/* Scan is in progress. Delay reporting. */
+					/* Scan complete will trigger bss_loss_work */
+					priv->delayed_link_loss = 1;
+					/* Also we're starting watchdog. */
+					queue_delayed_work(hw_priv->workqueue,
+							&priv->bss_loss_work, 10 * HZ);
+				}
+				break;
+			}
+			case WSM_EVENT_BSS_REGAINED:
+			{
+				sta_printk(XRADIO_DBG_WARN, "[CQM] BSS regained.\n");
+				priv->delayed_link_loss = 0;
+				spin_lock(&priv->bss_loss_lock);
+				priv->bss_loss_status = XRADIO_BSS_LOSS_NONE;
+				spin_unlock(&priv->bss_loss_lock);
+				cancel_delayed_work_sync(&priv->bss_loss_work);
+				cancel_delayed_work_sync(&priv->connection_loss_work);
+				break;
+			}
+			case WSM_EVENT_RADAR_DETECTED:
+				//STUB();
+				break;
+			case WSM_EVENT_RCPI_RSSI:
+			{
+				/* RSSI: signed Q8.0, RCPI: unsigned Q7.1
+				 * RSSI = RCPI / 2 - 110 */
+				int rcpiRssi = (int)(event->evt.eventData & 0xFF);
+				int cqm_evt;
+				if (priv->cqm_use_rssi)
+					rcpiRssi = (s8)rcpiRssi;
+				else
+					rcpiRssi =  rcpiRssi / 2 - 110;
+
+				cqm_evt = (rcpiRssi <= priv->cqm_rssi_thold) ?
+					NL80211_CQM_RSSI_THRESHOLD_EVENT_LOW :
+					NL80211_CQM_RSSI_THRESHOLD_EVENT_HIGH;
+				sta_printk(XRADIO_DBG_NIY, "[CQM] RSSI event: %d", rcpiRssi);
+				ieee80211_cqm_rssi_notify(priv->vif,
+					cqm_evt,
+					0,
+					GFP_KERNEL);
+				break;
+			}
+			case WSM_EVENT_BT_INACTIVE:
+				//STUB();
+				break;
+			case WSM_EVENT_BT_ACTIVE:
+				//STUB();
+				break;
+			case WSM_EVENT_INACTIVITY:
+			{
+				int link_id = ffs((u32)(event->evt.eventData)) - 1;
+				struct sk_buff *skb;
+			        struct ieee80211_mgmt *deauth;
+			        struct xradio_link_entry *entry = NULL;
+
+				sta_printk(XRADIO_DBG_WARN, "Inactivity Event Recieved for "
+						"link_id %d\n", link_id);
+				skb = dev_alloc_skb(sizeof(struct ieee80211_mgmt) + 64);
+				if (!skb)
+					break;
+				skb_reserve(skb, 64);
+				xrwl_unmap_link(priv, link_id);
+				deauth = (struct ieee80211_mgmt *)skb_put(skb, sizeof(struct ieee80211_mgmt));
+	                        WARN_ON(!deauth);
+	                        entry = &priv->link_id_db[link_id - 1];
+	                        deauth->duration = 0;
+	                        memcpy(deauth->da, priv->vif->addr, ETH_ALEN);
+	                        memcpy(deauth->sa, entry->mac/*priv->link_id_db[i].mac*/, ETH_ALEN);
+	                        memcpy(deauth->bssid, priv->vif->addr, ETH_ALEN);
+				deauth->frame_control = cpu_to_le16(IEEE80211_FTYPE_MGMT |
+	                                                                    IEEE80211_STYPE_DEAUTH |
+	                                                                    IEEE80211_FCTL_TODS);
+	                        deauth->u.deauth.reason_code = WLAN_REASON_DEAUTH_LEAVING;
+	                        deauth->seq_ctrl = 0;
+	                        ieee80211_rx_irqsafe(priv->hw, skb);
+				sta_printk(XRADIO_DBG_WARN, " Inactivity Deauth Frame sent for MAC SA %pM \t and DA %pM\n", deauth->sa, deauth->da);
+				queue_work(priv->hw_priv->workqueue, &priv->set_tim_work);
+				break;
+			}
+		case WSM_EVENT_PS_MODE_ERROR:
+			{
+				if (!priv->uapsd_info.uapsdFlags &&
+					(priv->user_pm_mode != WSM_PSM_PS))
+				{
+					struct wsm_set_pm pm = priv->powersave_mode;
+					int ret = 0;
+
+					priv->powersave_mode.pmMode = WSM_PSM_ACTIVE;
+					ret = xradio_set_pm (priv, &priv->powersave_mode);
+					if(ret)
+						priv->powersave_mode = pm;
+				}
+                                break;
+			}
+		}
+	}
+	mutex_unlock(&hw_priv->conf_mutex);
+	__xradio_free_event_queue(&list);
+}
+
+void xradio_bss_loss_work(struct work_struct *work)
+{
+	struct xradio_vif *priv =
+		container_of(work, struct xradio_vif, bss_loss_work.work);
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	int timeout; /* in beacons */
+
+
+	timeout = priv->cqm_link_loss_count - priv->cqm_beacon_loss_count;
+	/* Skip the confimration procedure in P2P case */
+	if (priv->vif->p2p)
+		goto report;
+
+	spin_lock(&priv->bss_loss_lock);
+	if (priv->bss_loss_status == XRADIO_BSS_LOSS_CONFIRMING) {
+		//do loss report next time.
+		priv->bss_loss_status = XRADIO_BSS_LOSS_CONFIRMED;
+		spin_unlock(&priv->bss_loss_lock);
+		//wait for more 1s to loss confirm.
+		queue_delayed_work(hw_priv->workqueue, &priv->bss_loss_work, 1 * HZ);
+		return;
+	} else if (priv->bss_loss_status == XRADIO_BSS_LOSS_NONE) {
+		spin_unlock(&priv->bss_loss_lock);
+		//link is alive.
+		cancel_delayed_work_sync(&priv->connection_loss_work);
+		return; 
+	} else if (priv->bss_loss_status == XRADIO_BSS_LOSS_CHECKING) {
+		/* it mean no confirming packets, just report loss. */
+	}
+	spin_unlock(&priv->bss_loss_lock);
+
+report:
+	if (priv->cqm_beacon_loss_count) {
+		sta_printk(XRADIO_DBG_WARN, "[CQM] Beacon loss.\n");
+		if (timeout <= 0)
+			timeout = 0;
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+		//ieee80211_cqm_beacon_miss_notify(priv->vif, GFP_KERNEL);
+#endif /* CONFIG_XRADIO_USE_EXTENSIONS */
+	} else {
+		timeout = 0;
+	}
+
+	cancel_delayed_work_sync(&priv->connection_loss_work);
+	queue_delayed_work(hw_priv->workqueue, &priv->connection_loss_work,
+	                   timeout * HZ / 10);
+
+	spin_lock(&priv->bss_loss_lock);
+	priv->bss_loss_status = XRADIO_BSS_LOSS_NONE;
+	spin_unlock(&priv->bss_loss_lock);
+}
+
+void xradio_connection_loss_work(struct work_struct *work)
+{
+	struct xradio_vif *priv =
+	  container_of(work, struct xradio_vif, connection_loss_work.work);
+	sta_printk(XRADIO_DBG_ERROR, "[CQM] if%d Reporting connection loss.\n", 
+	           priv->if_id);
+	ieee80211_connection_loss(priv->vif);
+}
+
+void xradio_tx_failure_work(struct work_struct *work)
+{
+	//struct xradio_vif *priv =
+	//	container_of(work, struct xradio_vif, tx_failure_work);
+	sta_printk(XRADIO_DBG_WARN, "[CQM] Reporting TX failure.\n");
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+	//ieee80211_cqm_tx_fail_notify(priv->vif, GFP_KERNEL);
+#else /* CONFIG_XRADIO_USE_EXTENSIONS */
+	//(void)priv;
+#endif /* CONFIG_XRADIO_USE_EXTENSIONS */
+}
+
+/* Internal API								*/
+int xradio_setup_mac(struct xradio_common *hw_priv)
+{
+	int ret = 0, if_id;
+
+
+	if (hw_priv->sdd) {
+		struct wsm_configuration cfg = {
+			.dot11StationId = &hw_priv->mac_addr[0],
+			.dpdData      = hw_priv->sdd->data,
+			.dpdData_size = hw_priv->sdd->size,
+		};
+		for (if_id = 0; if_id < xrwl_get_nr_hw_ifaces(hw_priv);
+		     if_id++) {
+			/* Set low-power mode. */
+			ret |= WARN_ON(wsm_configuration(hw_priv, &cfg,
+				       if_id));
+		}
+		/* wsm_configuration only once, so release it */
+		release_firmware(hw_priv->sdd);
+		hw_priv->sdd = NULL;
+	}
+
+	/* BUG:TX output power is not set untill config_xradio is called.
+	 * This would lead to 0 power set in fw and would effect scan & p2p-find
+	 * Setting to default value here from sdd which would be overwritten when
+	 * we make connection to AP.This value is used only during scan & p2p-ops
+	 * untill AP connection is made */
+	/*BUG:TX output power: Hardcoding to 20dbm if CCX is not enabled*/
+	/*TODO: This might change*/
+	if (!hw_priv->output_power)
+		hw_priv->output_power=20;
+	sta_printk(XRADIO_DBG_MSG, "%s output power %d\n",__func__,hw_priv->output_power);
+
+	return ret;
+}
+
+void xradio_pending_offchanneltx_work(struct work_struct *work)
+{
+	struct xradio_vif *priv =
+	container_of(work, struct xradio_vif, pending_offchanneltx_work.work);
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+
+
+	mutex_lock(&hw_priv->conf_mutex);
+#ifdef ROC_DEBUG
+	sta_printk(XRADIO_DBG_WARN, "OFFCHAN PEND IN\n");
+#endif
+	xradio_disable_listening(priv);
+	hw_priv->roc_if_id = -1;
+#ifdef ROC_DEBUG
+	sta_printk(XRADIO_DBG_WARN, "OFFCHAN PEND OUT\n");
+#endif
+	up(&hw_priv->scan.lock);
+	mutex_unlock(&hw_priv->conf_mutex);
+}
+
+void xradio_offchannel_work(struct work_struct *work)
+{
+	struct xradio_vif *priv =
+		container_of(work, struct xradio_vif, offchannel_work);
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	u8 queueId = xradio_queue_get_queue_id(hw_priv->pending_frame_id);
+	struct xradio_queue *queue = &hw_priv->tx_queue[queueId];
+
+
+	BUG_ON(queueId >= 4);
+	BUG_ON(!hw_priv->channel);
+
+	if (unlikely(down_trylock(&hw_priv->scan.lock))) {
+		int ret;
+		sta_printk(XRADIO_DBG_ERROR, "xradio_offchannel_work***** drop frame\n");
+		ret = xradio_queue_remove(queue, hw_priv->pending_frame_id);
+		if (ret)
+			sta_printk(XRADIO_DBG_ERROR, "xradio_offchannel_work: "
+				       "queue_remove failed %d\n", ret);
+		wsm_unlock_tx(hw_priv);
+		//workaround by yangfh
+		up(&hw_priv->scan.lock);
+		ieee80211_connection_loss(priv->vif);
+		sta_printk(XRADIO_DBG_ERROR,"lock %d\n", hw_priv->scan.lock.count);
+		
+		return;
+	}
+	mutex_lock(&hw_priv->conf_mutex);
+#ifdef ROC_DEBUG
+	sta_printk(XRADIO_DBG_WARN, "OFFCHAN WORK IN %d\n", priv->if_id);
+#endif
+	hw_priv->roc_if_id = priv->if_id;
+	if (likely(!priv->join_status)) {
+		wsm_vif_flush_tx(priv);
+		xradio_enable_listening(priv, hw_priv->channel);
+		/* xradio_update_filtering(priv); */
+	}
+	if (unlikely(!priv->join_status))
+		xradio_queue_remove(queue, hw_priv->pending_frame_id);
+	else
+		xradio_queue_requeue(queue, hw_priv->pending_frame_id, false);
+
+	queue_delayed_work(hw_priv->workqueue,
+			&priv->pending_offchanneltx_work, 204 * HZ/1000);
+#ifdef ROC_DEBUG
+	sta_printk(XRADIO_DBG_WARN, "OFFCHAN WORK OUT %d\n", priv->if_id);
+#endif
+	mutex_unlock(&hw_priv->conf_mutex);
+	wsm_unlock_tx(hw_priv);
+}
+
+void xradio_join_work(struct work_struct *work)
+{
+	struct xradio_vif *priv =
+		container_of(work, struct xradio_vif, join_work);
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	u8 queueId = xradio_queue_get_queue_id(hw_priv->pending_frame_id);
+	struct xradio_queue *queue = &hw_priv->tx_queue[queueId];
+	const struct xradio_txpriv *txpriv = NULL;
+	struct sk_buff *skb = NULL;
+	const struct wsm_tx *wsm;
+	const struct ieee80211_hdr *frame;
+	const u8 *bssid;
+	struct cfg80211_bss *bss;
+	const u8 *ssidie;
+	const u8 *dtimie;
+	const struct ieee80211_tim_ie *tim = NULL;
+	struct wsm_protected_mgmt_policy mgmt_policy;
+	//struct wsm_reset reset = {
+	//	.reset_statistics = true,
+	//};
+
+
+
+	BUG_ON(queueId >= 4);
+	if (xradio_queue_get_skb(queue,	hw_priv->pending_frame_id,
+			&skb, &txpriv)) {
+		wsm_unlock_tx(hw_priv);
+		return;
+	}
+	wsm = (struct wsm_tx *)&skb->data[0];
+	frame = (struct ieee80211_hdr *)&skb->data[txpriv->offset];
+	bssid = &frame->addr1[0]; /* AP SSID in a 802.11 frame */
+
+	BUG_ON(!wsm);
+	BUG_ON(!hw_priv->channel);
+
+	if (unlikely(priv->join_status)) {
+		sta_printk(XRADIO_DBG_WARN, "%s, pre join_status=%d.\n",
+		          __func__, priv->join_status);
+		wsm_lock_tx(hw_priv);
+		xradio_unjoin_work(&priv->unjoin_work);
+	}
+
+	cancel_delayed_work_sync(&priv->join_timeout);
+
+	bss = cfg80211_get_bss(hw_priv->hw->wiphy, hw_priv->channel,
+			bssid, NULL, 0, 0, 0);
+	if (!bss) {
+		xradio_queue_remove(queue, hw_priv->pending_frame_id);
+		wsm_unlock_tx(hw_priv);
+		return;
+	}
+	ssidie = cfg80211_find_ie(WLAN_EID_SSID,
+		bss->ies->data,
+		bss->ies->len);
+	dtimie = cfg80211_find_ie(WLAN_EID_TIM,
+		bss->ies->data,
+		bss->ies->len);
+	if (dtimie)
+		tim = (struct ieee80211_tim_ie *)&dtimie[2];
+
+	mutex_lock(&hw_priv->conf_mutex);
+	{
+		struct wsm_join join = {
+			.mode = (bss->capability & WLAN_CAPABILITY_IBSS) ?
+				WSM_JOIN_MODE_IBSS : WSM_JOIN_MODE_BSS,
+			/* default changed to LONG, by HuangLu, fix 2/5.5/11m tx fail*/
+			.preambleType = WSM_JOIN_PREAMBLE_LONG,
+			.probeForJoin = 1,
+			/* dtimPeriod will be updated after association */
+			.dtimPeriod = 1,
+			.beaconInterval = bss->beacon_interval,
+		};
+
+		if (priv->if_id)
+			join.flags |= WSM_FLAG_MAC_INSTANCE_1;
+		else
+			join.flags &= ~WSM_FLAG_MAC_INSTANCE_1;
+
+		/* BT Coex related changes */
+		if (hw_priv->is_BT_Present) {
+			if (((hw_priv->conf_listen_interval * 100) %
+					bss->beacon_interval) == 0)
+				priv->listen_interval =
+					((hw_priv->conf_listen_interval * 100) /
+					bss->beacon_interval);
+			else
+				priv->listen_interval =
+					((hw_priv->conf_listen_interval * 100) /
+					bss->beacon_interval + 1);
+		}
+
+		if (tim && tim->dtim_period > 1) {
+			join.dtimPeriod = tim->dtim_period;
+			priv->join_dtim_period = tim->dtim_period;
+		}
+		priv->beacon_int = bss->beacon_interval;
+		sta_printk(XRADIO_DBG_NIY, "Join DTIM: %d, interval: %d\n",
+				join.dtimPeriod, priv->beacon_int);
+
+		hw_priv->is_go_thru_go_neg = false;
+		join.channelNumber = hw_priv->channel->hw_value;
+
+		/* basicRateSet will be updated after association.
+		Currently these values are hardcoded */
+		if (hw_priv->channel->band == NL80211_BAND_5GHZ) {
+			join.band = WSM_PHY_BAND_5G;
+			join.basicRateSet = 64; /*6 mbps*/
+		}else{
+			join.band = WSM_PHY_BAND_2_4G;
+			join.basicRateSet = 7; /*1, 2, 5.5 mbps*/
+		}
+		memcpy(&join.bssid[0], bssid, sizeof(join.bssid));
+		memcpy(&priv->join_bssid[0], bssid, sizeof(priv->join_bssid));
+
+		if (ssidie) {
+			join.ssidLength = ssidie[1];
+			if (WARN_ON(join.ssidLength > sizeof(join.ssid)))
+				join.ssidLength = sizeof(join.ssid);
+			memcpy(&join.ssid[0], &ssidie[2], join.ssidLength);
+			if(strstr(&join.ssid[0],"5.1.4"))
+				msleep(200);
+#ifdef ROAM_OFFLOAD
+			if((priv->vif->type == NL80211_IFTYPE_STATION)) {
+				priv->ssid_length = join.ssidLength;
+				memcpy(priv->ssid, &join.ssid[0], priv->ssid_length);
+			}
+#endif /*ROAM_OFFLOAD*/
+		}
+
+		if (priv->vif->p2p) {
+			join.flags |= WSM_JOIN_FLAGS_P2P_GO;
+			join.basicRateSet =
+				xradio_rate_mask_to_wsm(hw_priv, 0xFF0);
+		}
+
+		wsm_flush_tx(hw_priv);
+
+		/* Queue unjoin if not associated in 3 sec. */
+		queue_delayed_work(hw_priv->workqueue,
+			&priv->join_timeout, 3 * HZ);
+		/*Stay Awake for Join Timeout*/
+		xradio_pm_stay_awake(&hw_priv->pm_state, 3 * HZ);
+
+		xradio_disable_listening(priv);
+
+		//WARN_ON(wsm_reset(hw_priv, &reset, priv->if_id));
+		WARN_ON(wsm_set_operational_mode(hw_priv, &defaultoperationalmode, priv->if_id));
+		WARN_ON(wsm_set_block_ack_policy(hw_priv,
+			0, hw_priv->ba_tid_mask, priv->if_id));
+		spin_lock_bh(&hw_priv->ba_lock);
+		hw_priv->ba_ena = false;
+		hw_priv->ba_cnt = 0;
+		hw_priv->ba_acc = 0;
+		hw_priv->ba_hist = 0;
+		hw_priv->ba_cnt_rx = 0;
+		hw_priv->ba_acc_rx = 0;
+		spin_unlock_bh(&hw_priv->ba_lock);
+
+		mgmt_policy.protectedMgmtEnable = 0;
+		mgmt_policy.unprotectedMgmtFramesAllowed = 1;
+		mgmt_policy.encryptionForAuthFrame = 1;
+		wsm_set_protected_mgmt_policy(hw_priv, &mgmt_policy, priv->if_id);
+
+		if (wsm_join(hw_priv, &join, priv->if_id)) {
+			memset(&priv->join_bssid[0],
+				0, sizeof(priv->join_bssid));
+			xradio_queue_remove(queue, hw_priv->pending_frame_id);
+			cancel_delayed_work_sync(&priv->join_timeout);
+		} else {
+			/* Upload keys */
+			xradio_queue_requeue(queue, hw_priv->pending_frame_id,
+						true);
+			priv->join_status = XRADIO_JOIN_STATUS_STA;
+
+			/* Due to beacon filtering it is possible that the
+			 * AP's beacon is not known for the mac80211 stack.
+			 * Disable filtering temporary to make sure the stack
+			 * receives at least one */
+			priv->disable_beacon_filter = true;
+
+		}
+		xradio_update_filtering(priv);
+	}
+	mutex_unlock(&hw_priv->conf_mutex);
+	cfg80211_put_bss(hw_priv->hw->wiphy,bss);
+	wsm_unlock_tx(hw_priv);
+}
+
+void xradio_join_timeout(struct work_struct *work)
+{
+	struct xradio_vif *priv =
+		container_of(work, struct xradio_vif, join_timeout.work);
+	sta_printk(XRADIO_DBG_WARN, "[WSM] Issue unjoin command (TMO).\n");
+	wsm_lock_tx(priv->hw_priv);
+	xradio_unjoin_work(&priv->unjoin_work);
+}
+
+void xradio_unjoin_work(struct work_struct *work)
+{
+	struct xradio_vif *priv =
+		container_of(work, struct xradio_vif, unjoin_work);
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	
+	struct wsm_reset reset = {
+		.reset_statistics = true,
+	};
+	bool is_htcapie = false;
+	int i;
+	struct xradio_vif *tmp_priv;
+
+	//add by yangfh.
+	hw_priv->connet_time[priv->if_id] = 0;
+#ifdef AP_HT_COMPAT_FIX
+	priv->ht_compat_det &= ~1;
+	priv->ht_compat_cnt = 0;
+#endif
+
+	del_timer_sync(&hw_priv->ba_timer);
+	mutex_lock(&hw_priv->conf_mutex);
+	if (unlikely(atomic_read(&hw_priv->scan.in_progress))) {
+		if (atomic_xchg(&priv->delayed_unjoin, 1)) {
+			sta_printk(XRADIO_DBG_NIY, 
+				"%s: Delayed unjoin "
+				"is already scheduled.\n",
+				__func__);
+			wsm_unlock_tx(hw_priv);
+		}
+		mutex_unlock(&hw_priv->conf_mutex);
+		return;
+	}
+
+	if (priv->join_status &&
+			priv->join_status > XRADIO_JOIN_STATUS_STA) {
+		sta_printk(XRADIO_DBG_ERROR, 
+				"%s: Unexpected: join status: %d\n",
+				__func__, priv->join_status);
+		BUG_ON(1);
+	}
+	if (priv->join_status) {
+		cancel_work_sync(&priv->update_filtering_work);
+		cancel_work_sync(&priv->set_beacon_wakeup_period_work);
+		memset(&priv->join_bssid[0], 0, sizeof(priv->join_bssid));
+		priv->join_status = XRADIO_JOIN_STATUS_PASSIVE;
+
+		/* Unjoin is a reset. */
+		wsm_flush_tx(hw_priv);
+		WARN_ON(wsm_keep_alive_period(hw_priv, 0, priv->if_id));
+		WARN_ON(wsm_reset(hw_priv, &reset, priv->if_id));
+		WARN_ON(wsm_set_operational_mode(hw_priv, &defaultoperationalmode, priv->if_id));
+		WARN_ON(wsm_set_output_power(hw_priv,
+			hw_priv->output_power * 10, priv->if_id));
+		priv->join_dtim_period = 0;
+		priv->cipherType = 0;
+		WARN_ON(xradio_setup_mac_pvif(priv));
+		xradio_free_event_queue(hw_priv);
+		cancel_work_sync(&hw_priv->event_handler);
+		cancel_delayed_work_sync(&priv->connection_loss_work);
+		WARN_ON(wsm_set_block_ack_policy(hw_priv,
+			0, hw_priv->ba_tid_mask, priv->if_id));
+		priv->disable_beacon_filter = false;
+		xradio_update_filtering(priv);
+		priv->setbssparams_done = false;
+		memset(&priv->association_mode, 0,
+			sizeof(priv->association_mode));
+		memset(&priv->bss_params, 0, sizeof(priv->bss_params));
+		memset(&priv->firmware_ps_mode, 0,
+			sizeof(priv->firmware_ps_mode));
+		priv->htcap = false;
+		xradio_for_each_vif(hw_priv, tmp_priv, i) {
+			if (!tmp_priv)
+				continue;
+			if ((tmp_priv->join_status == XRADIO_JOIN_STATUS_STA) && tmp_priv->htcap)
+				is_htcapie = true;
+		}
+
+		if (is_htcapie) {
+			hw_priv->vif0_throttle = XRWL_HOST_VIF0_11N_THROTTLE;
+			hw_priv->vif1_throttle = XRWL_HOST_VIF1_11N_THROTTLE;
+			sta_printk(XRADIO_DBG_NIY, "UNJOIN HTCAP 11N %d\n",hw_priv->vif0_throttle);
+		} else {
+			hw_priv->vif0_throttle = XRWL_HOST_VIF0_11BG_THROTTLE;
+			hw_priv->vif1_throttle = XRWL_HOST_VIF1_11BG_THROTTLE;
+			sta_printk(XRADIO_DBG_NIY, "UNJOIN 11BG %d\n",hw_priv->vif0_throttle);
+		}
+		sta_printk(XRADIO_DBG_NIY, "Unjoin.\n");
+	}
+	mutex_unlock(&hw_priv->conf_mutex);
+	wsm_unlock_tx(hw_priv);
+}
+
+int xradio_enable_listening(struct xradio_vif *priv,
+				struct ieee80211_channel *chan)
+{
+	/* TODO:COMBO: Channel is common to HW currently in mac80211.
+	Change the code below once channel is made per VIF */
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	struct wsm_start start = {
+		.mode = WSM_START_MODE_P2P_DEV | (priv->if_id << 4),
+		.band = (chan->band == NL80211_BAND_5GHZ) ?
+				WSM_PHY_BAND_5G : WSM_PHY_BAND_2_4G,
+		.channelNumber = chan->hw_value,
+		.beaconInterval = 100,
+		.DTIMPeriod = 1,
+		.probeDelay = 0,
+		.basicRateSet = 0x0F,
+	};
+
+
+	if(priv->if_id != 2) {
+		WARN_ON(priv->join_status > XRADIO_JOIN_STATUS_MONITOR);
+		return 0;
+	}
+	if (priv->join_status == XRADIO_JOIN_STATUS_MONITOR)
+		return 0;
+	if (priv->join_status == XRADIO_JOIN_STATUS_PASSIVE)
+		priv->join_status = XRADIO_JOIN_STATUS_MONITOR;
+
+	WARN_ON(priv->join_status > XRADIO_JOIN_STATUS_MONITOR);
+
+	return wsm_start(hw_priv, &start, XRWL_GENERIC_IF_ID);
+}
+
+int xradio_disable_listening(struct xradio_vif *priv)
+{
+	int ret;
+	struct wsm_reset reset = {
+		.reset_statistics = true,
+	};
+
+
+	if(priv->if_id != 2) {
+		WARN_ON(priv->join_status > XRADIO_JOIN_STATUS_MONITOR);
+        return 0;
+	}
+	priv->join_status = XRADIO_JOIN_STATUS_PASSIVE;
+
+	WARN_ON(priv->join_status > XRADIO_JOIN_STATUS_MONITOR);
+
+	if (priv->hw_priv->roc_if_id == -1)
+		return 0;
+
+	ret = wsm_reset(priv->hw_priv, &reset, XRWL_GENERIC_IF_ID);
+	return ret;
+}
+
+/* TODO:COMBO:UAPSD will be supported only on one interface */
+int xradio_set_uapsd_param(struct xradio_vif *priv,
+				const struct wsm_edca_params *arg)
+{
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	int ret;
+	u16 uapsdFlags = 0;
+
+
+	/* Here's the mapping AC [queue, bit]
+	VO [0,3], VI [1, 2], BE [2, 1], BK [3, 0]*/
+
+	if (arg->params[0].uapsdEnable)
+		uapsdFlags |= 1 << 3;
+
+	if (arg->params[1].uapsdEnable)
+		uapsdFlags |= 1 << 2;
+
+	if (arg->params[2].uapsdEnable)
+		uapsdFlags |= 1 << 1;
+
+	if (arg->params[3].uapsdEnable)
+		uapsdFlags |= 1;
+
+	/* Currently pseudo U-APSD operation is not supported, so setting
+	* MinAutoTriggerInterval, MaxAutoTriggerInterval and
+	* AutoTriggerStep to 0 */
+
+	priv->uapsd_info.uapsdFlags = cpu_to_le16(uapsdFlags);
+	priv->uapsd_info.minAutoTriggerInterval = 0;
+	priv->uapsd_info.maxAutoTriggerInterval = 0;
+	priv->uapsd_info.autoTriggerStep = 0;
+
+	ret = wsm_set_uapsd_info(hw_priv, &priv->uapsd_info,
+				 priv->if_id);
+	return ret;
+}
+
+void xradio_ba_work(struct work_struct *work)
+{
+	struct xradio_common *hw_priv =
+		container_of(work, struct xradio_common, ba_work);
+	u8 tx_ba_tid_mask;
+
+
+	/* TODO:COMBO: reenable this part of code */
+/*	if (priv->join_status != XRADIO_JOIN_STATUS_STA)
+		return;
+	if (!priv->setbssparams_done)
+		return;*/
+
+	sta_printk(XRADIO_DBG_WARN, "BA work****\n");
+	spin_lock_bh(&hw_priv->ba_lock);
+//	tx_ba_tid_mask = hw_priv->ba_ena ? hw_priv->ba_tid_mask : 0;
+	tx_ba_tid_mask = hw_priv->ba_tid_mask;
+	spin_unlock_bh(&hw_priv->ba_lock);
+
+	wsm_lock_tx(hw_priv);
+
+	WARN_ON(wsm_set_block_ack_policy(hw_priv,
+		tx_ba_tid_mask, hw_priv->ba_tid_mask, -1)); /*TODO:COMBO*/
+
+	wsm_unlock_tx(hw_priv);
+}
+
+void xradio_ba_timer(struct timer_list *t)
+{
+	struct xradio_common *hw_priv = from_timer(hw_priv, t, ba_timer);
+	bool ba_ena;
+
+
+	spin_lock_bh(&hw_priv->ba_lock);
+
+	if (atomic_read(&hw_priv->scan.in_progress)) {
+		hw_priv->ba_cnt = 0;
+		hw_priv->ba_acc = 0;
+		hw_priv->ba_cnt_rx = 0;
+		hw_priv->ba_acc_rx = 0;
+		goto skip_statistic_update;
+	}
+
+	if (hw_priv->ba_cnt >= XRADIO_BLOCK_ACK_CNT &&
+		(hw_priv->ba_acc / hw_priv->ba_cnt >= XRADIO_BLOCK_ACK_THLD ||
+		(hw_priv->ba_cnt_rx >= XRADIO_BLOCK_ACK_CNT &&
+		hw_priv->ba_acc_rx / hw_priv->ba_cnt_rx >=
+			XRADIO_BLOCK_ACK_THLD)))
+		ba_ena = true;
+	else
+		ba_ena = false;
+
+	hw_priv->ba_cnt = 0;
+	hw_priv->ba_acc = 0;
+	hw_priv->ba_cnt_rx = 0;
+	hw_priv->ba_acc_rx = 0;
+
+	if (ba_ena != hw_priv->ba_ena) {
+		if (ba_ena || ++hw_priv->ba_hist >= XRADIO_BLOCK_ACK_HIST) {
+			hw_priv->ba_ena = ba_ena;
+			hw_priv->ba_hist = 0;
+#if 0
+			sta_printk(XRADIO_DBG_NIY, "%s block ACK:\n",
+				ba_ena ? "enable" : "disable");
+			queue_work(hw_priv->workqueue, &hw_priv->ba_work);
+#endif
+		}
+	} else if (hw_priv->ba_hist)
+		--hw_priv->ba_hist;
+
+skip_statistic_update:
+	spin_unlock_bh(&hw_priv->ba_lock);
+}
+
+int xradio_vif_setup(struct xradio_vif *priv)
+{
+	struct xradio_common *hw_priv = priv->hw_priv;
+	int ret = 0;
+
+
+	//reset channel change flag, yangfh 2015-5-15 17:12:14
+	hw_priv->channel_changed  = 0;
+	/* Setup per vif workitems and locks */
+	spin_lock_init(&priv->vif_lock);
+	INIT_WORK(&priv->join_work, xradio_join_work);
+	INIT_DELAYED_WORK(&priv->join_timeout, xradio_join_timeout);
+	INIT_WORK(&priv->unjoin_work, xradio_unjoin_work);
+	INIT_WORK(&priv->wep_key_work, xradio_wep_key_work);
+	INIT_WORK(&priv->offchannel_work, xradio_offchannel_work);
+	INIT_DELAYED_WORK(&priv->bss_loss_work, xradio_bss_loss_work);
+	INIT_DELAYED_WORK(&priv->connection_loss_work, xradio_connection_loss_work);
+	priv->bss_loss_status = XRADIO_BSS_LOSS_NONE;
+	spin_lock_init(&priv->bss_loss_lock);
+	INIT_WORK(&priv->tx_failure_work, xradio_tx_failure_work);
+	spin_lock_init(&priv->ps_state_lock);
+	INIT_DELAYED_WORK(&priv->set_cts_work, xradio_set_cts_work);
+	INIT_WORK(&priv->set_tim_work, xradio_set_tim_work);
+	INIT_WORK(&priv->multicast_start_work, xradio_multicast_start_work);
+	INIT_WORK(&priv->multicast_stop_work, xradio_multicast_stop_work);
+	INIT_WORK(&priv->link_id_work, xradio_link_id_work);
+	INIT_DELAYED_WORK(&priv->link_id_gc_work, xradio_link_id_gc_work);
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+	INIT_WORK(&priv->linkid_reset_work, xradio_link_id_reset);
+#endif
+	INIT_WORK(&priv->update_filtering_work, xradio_update_filtering_work);
+	INIT_DELAYED_WORK(&priv->pending_offchanneltx_work,
+			xradio_pending_offchanneltx_work);
+	INIT_WORK(&priv->set_beacon_wakeup_period_work,
+		xradio_set_beacon_wakeup_period_work);
+#ifdef AP_HT_CAP_UPDATE
+        INIT_WORK(&priv->ht_oper_update_work, xradio_ht_oper_update_work);
+#endif
+	timer_setup(&priv->mcast_timeout, xradio_mcast_timeout, 0);
+	priv->setbssparams_done = false;
+	priv->power_set_true = 0;
+	priv->user_power_set_true = 0;
+	priv->user_pm_mode = 0;
+
+	/* Initialising the broadcast filter */
+	memset(priv->broadcast_filter.MacAddr, 0xFF, ETH_ALEN);
+	priv->broadcast_filter.nummacaddr = 1;
+	priv->broadcast_filter.address_mode = 1;
+	priv->broadcast_filter.filter_mode = 1;
+	priv->htcap = false;
+#ifdef AP_HT_COMPAT_FIX
+	priv->ht_compat_det = 0;
+	priv->ht_compat_cnt = 0;
+#endif
+
+	sta_printk(XRADIO_DBG_ALWY, "!!!%s: id=%d, type=%d, p2p=%d\n",
+			__func__, priv->if_id, priv->vif->type, priv->vif->p2p);
+
+	atomic_set(&priv->enabled, 1);
+
+		/* default EDCA */
+		WSM_EDCA_SET(&priv->edca, 0, 0x0002, 0x0003, 0x0007,
+				47, 0xc8, false);
+		WSM_EDCA_SET(&priv->edca, 1, 0x0002, 0x0007, 0x000f,
+				94, 0xc8, false);
+
+//		if(priv->vif->p2p == true) {
+			WSM_EDCA_SET(&priv->edca, 2, 0x0002, 0x0003, 0x0007,
+				0, 0xc8, false);
+			sta_printk(XRADIO_DBG_MSG, "EDCA params Best effort for sta/p2p is " \
+				 "aifs=%u, cw_min=%u, cw_max=%u \n",
+				priv->edca.params[2].aifns, priv->edca.params[2].cwMin,
+				 priv->edca.params[2].cwMax);
+#if 0					 
+		}else {
+			WSM_EDCA_SET(&priv->edca, 2, 0x0003, 0x000f, 0x03ff,
+				0, 0xc8, false);
+			sta_printk(XRADIO_DBG_MSG, "EDCA params Best effort for sta is " \
+				 "aifs=%u, cw_min=%u, cw_max=%u \n",
+				priv->edca.params[2].aifns, priv->edca.params[2].cwMin,
+				 priv->edca.params[2].cwMax);
+		}
+#endif
+		WSM_EDCA_SET(&priv->edca, 3, 0x0007, 0x000f, 0x03ff,
+				0, 0xc8, false);
+
+		ret = wsm_set_edca_params(hw_priv, &priv->edca, priv->if_id);
+		if (WARN_ON(ret))
+			goto out;
+
+		ret = xradio_set_uapsd_param(priv, &priv->edca);
+		if (WARN_ON(ret))
+			goto out;
+
+		memset(priv->bssid, ~0, ETH_ALEN);
+		priv->wep_default_key_id = -1;
+		priv->cipherType = 0;
+		priv->cqm_link_loss_count   = XRADIO_LINK_LOSS_THOLD_DEF;
+		priv->cqm_beacon_loss_count = XRADIO_BSS_LOSS_THOLD_DEF;
+
+		/* Temporary configuration - beacon filter table */
+		__xradio_bf_configure(priv);
+
+out:
+	return ret;
+}
+
+int xradio_setup_mac_pvif(struct xradio_vif *priv)
+{
+	int ret = 0;
+	/* NOTE: There is a bug in FW: it reports signal
+	* as RSSI if RSSI subscription is enabled.
+	* It's not enough to set WSM_RCPI_RSSI_USE_RSSI. */
+	/* NOTE2: RSSI based reports have been switched to RCPI, since
+	* FW has a bug and RSSI reported values are not stable,
+	* what can leads to signal level oscilations in user-end applications */
+	struct wsm_rcpi_rssi_threshold threshold = {
+		.rssiRcpiMode = WSM_RCPI_RSSI_THRESHOLD_ENABLE |
+		WSM_RCPI_RSSI_DONT_USE_UPPER |
+		WSM_RCPI_RSSI_DONT_USE_LOWER,
+		.rollingAverageCount = 16,
+	};
+
+
+	/* Remember the decission here to make sure, we will handle
+	 * the RCPI/RSSI value correctly on WSM_EVENT_RCPI_RSS */
+	if (threshold.rssiRcpiMode & WSM_RCPI_RSSI_USE_RSSI)
+		priv->cqm_use_rssi = true;
+
+
+	/* Configure RSSI/SCPI reporting as RSSI. */
+	ret = wsm_set_rcpi_rssi_threshold(priv->hw_priv, &threshold, priv->if_id);
+	return ret;
+}
+
+void xradio_rem_chan_timeout(struct work_struct *work)
+{
+	struct xradio_common *hw_priv =
+		container_of(work, struct xradio_common, rem_chan_timeout.work);
+	int ret, if_id;
+	struct xradio_vif *priv;
+
+
+#ifdef TES_P2P_0002_ROC_RESTART
+	if(TES_P2P_0002_state == TES_P2P_0002_STATE_GET_PKTID) {
+		sta_printk(XRADIO_DBG_WARN, "[Restart rem_chan_timeout:Timeout]\n");
+		return;
+	}
+#endif
+
+	if (atomic_read(&hw_priv->remain_on_channel) == 0) {
+		return;
+	}
+	ieee80211_remain_on_channel_expired(hw_priv->hw);
+
+	mutex_lock(&hw_priv->conf_mutex);
+	if_id = hw_priv->roc_if_id;
+#ifdef ROC_DEBUG
+	sta_printk(XRADIO_DBG_ERROR, "ROC TO IN %d\n", if_id);
+#endif
+	priv = __xrwl_hwpriv_to_vifpriv(hw_priv, if_id);
+	ret = WARN_ON(__xradio_flush(hw_priv, false, if_id));
+	if (!ret) {
+		xradio_disable_listening(priv);
+	}
+	atomic_set(&hw_priv->remain_on_channel, 0);
+	hw_priv->roc_if_id = -1;
+
+#ifdef ROC_DEBUG
+	sta_printk(XRADIO_DBG_ERROR, "ROC TO OUT %d\n", if_id);
+#endif
+
+	mutex_unlock(&hw_priv->conf_mutex);
+	up(&hw_priv->scan.lock);
+}
+const u8 *xradio_get_ie(u8 *start, size_t len, u8 ie)
+{
+	u8 *end, *pos;
+
+
+	pos = start;
+	if (pos == NULL)
+		return NULL;
+	end = pos + len;
+
+	while (pos + 1 < end) {
+		if (pos + 2 + pos[1] > end)
+			break;
+		if (pos[0] == ie)
+			return pos;
+		pos += 2 + pos[1];
+	}
+
+	return NULL;
+}
+
+/**
+ * xradio_set_macaddrfilter -called when tesmode command
+ * is for setting mac address filter
+ *
+ * @hw: the hardware
+ * @data: incoming data
+ *
+ * Returns: 0 on success or non zero value on failure
+ */
+int xradio_set_macaddrfilter(struct xradio_common *hw_priv, struct xradio_vif *priv, u8 *data)
+{
+	struct wsm_mac_addr_filter *mac_addr_filter =  NULL;
+	struct wsm_mac_addr_info *addr_info = NULL;
+	u8 action_mode = 0, no_of_mac_addr = 0, i = 0;
+	int ret = 0;
+	u16 macaddrfiltersize = 0;
+
+
+	/* Retrieving Action Mode */
+	action_mode = data[0];
+	/* Retrieving number of address entries */
+	no_of_mac_addr = data[1];
+
+	addr_info = (struct wsm_mac_addr_info *)&data[2];
+
+	/* Computing sizeof Mac addr filter */
+	macaddrfiltersize =  sizeof(*mac_addr_filter) + \
+			(no_of_mac_addr * sizeof(struct wsm_mac_addr_info));
+
+	mac_addr_filter = kzalloc(macaddrfiltersize, GFP_KERNEL);
+	if (!mac_addr_filter) {
+		ret = -ENOMEM;
+		goto exit_p;
+	}
+	mac_addr_filter->action_mode = action_mode;
+	mac_addr_filter->numfilter = no_of_mac_addr;
+
+	for (i = 0; i < no_of_mac_addr; i++) {
+		mac_addr_filter->macaddrfilter[i].address_mode = \
+						addr_info[i].address_mode;
+		memcpy(mac_addr_filter->macaddrfilter[i].MacAddr, \
+				addr_info[i].MacAddr , ETH_ALEN);
+		mac_addr_filter->macaddrfilter[i].filter_mode = \
+						addr_info[i].filter_mode;
+	}
+	ret = WARN_ON(wsm_write_mib(hw_priv, WSM_MIB_ID_MAC_ADDR_FILTER, \
+					 mac_addr_filter, macaddrfiltersize, priv->if_id));
+
+	kfree(mac_addr_filter);
+exit_p:
+	return ret;
+}
+
+/**
+ * xradio_set_arpreply -called for creating and
+ * configuring arp response template frame
+ *
+ * @hw: the hardware
+ *
+ * Returns: 0 on success or non zero value on failure
+ */
+int xradio_set_arpreply(struct ieee80211_hw *hw, struct ieee80211_vif *vif)
+{
+	struct xradio_vif *priv = xrwl_get_vif_from_ieee80211(vif);
+	struct xradio_common *hw_priv = (struct xradio_common *)hw->priv;
+	u32 framehdrlen, encrypthdr, encrypttailsize, framebdylen = 0;
+	bool encrypt = false;
+	int ret = 0;
+	u8 *template_frame = NULL;
+	struct ieee80211_hdr_3addr *dot11hdr = NULL;
+	struct ieee80211_snap_hdr *snaphdr = NULL;
+	struct arphdr *arp_hdr = NULL;
+
+
+	template_frame = kzalloc(MAX_ARP_REPLY_TEMPLATE_SIZE, GFP_KERNEL);
+	if (!template_frame) {
+		sta_printk(XRADIO_DBG_ERROR, "Template frame memory failed\n");
+		ret = -ENOMEM;
+		goto exit_p;
+	}
+	dot11hdr = (struct ieee80211_hdr_3addr *)&template_frame[4];
+	
+	framehdrlen = sizeof(*dot11hdr);
+	if ((priv->vif->type == NL80211_IFTYPE_AP) && priv->vif->p2p)
+	        priv->cipherType = WLAN_CIPHER_SUITE_CCMP;
+	switch (priv->cipherType) {
+	
+	case WLAN_CIPHER_SUITE_WEP40:
+	case WLAN_CIPHER_SUITE_WEP104:
+		sta_printk(XRADIO_DBG_NIY, "WEP\n");
+		encrypthdr = WEP_ENCRYPT_HDR_SIZE;
+		encrypttailsize = WEP_ENCRYPT_TAIL_SIZE;
+		encrypt = 1;
+		break;
+	
+	
+	case WLAN_CIPHER_SUITE_TKIP:
+		sta_printk(XRADIO_DBG_NIY, "WPA\n");
+		encrypthdr = WPA_ENCRYPT_HDR_SIZE;
+		encrypttailsize = WPA_ENCRYPT_TAIL_SIZE;
+		encrypt = 1;
+		break;
+	
+	case WLAN_CIPHER_SUITE_CCMP:
+		sta_printk(XRADIO_DBG_NIY, "WPA2\n");
+		encrypthdr = WPA2_ENCRYPT_HDR_SIZE;
+		encrypttailsize = WPA2_ENCRYPT_TAIL_SIZE;
+		encrypt = 1;
+		break;
+	
+	case WLAN_CIPHER_SUITE_SMS4:
+		sta_printk(XRADIO_DBG_NIY, "WAPI\n");
+		encrypthdr = WAPI_ENCRYPT_HDR_SIZE;
+		encrypttailsize = WAPI_ENCRYPT_TAIL_SIZE;
+		encrypt = 1;
+		break;
+	
+	default:
+		encrypthdr = 0;
+		encrypttailsize = 0;
+		encrypt = 0;
+		break;
+	}
+
+	framehdrlen += encrypthdr;
+	/* Filling the 802.11 Hdr */
+	dot11hdr->frame_control = cpu_to_le16(IEEE80211_FTYPE_DATA);
+	if (priv->vif->type == NL80211_IFTYPE_STATION)
+		dot11hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_TODS);
+	else
+		dot11hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_FROMDS);
+	
+	if (encrypt)
+		dot11hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_WEP);
+	
+	if (priv->vif->bss_conf.qos) {
+		sta_printk(XRADIO_DBG_NIY, "QOS Enabled\n");
+		dot11hdr->frame_control |= cpu_to_le16(IEEE80211_QOS_DATAGRP);
+		*(u16 *)(dot11hdr + 1) = 0x0;
+		framehdrlen += 2;
+	} else {
+		dot11hdr->frame_control |= cpu_to_le16(IEEE80211_STYPE_DATA);
+	}
+	
+	memcpy(dot11hdr->addr1, priv->vif->bss_conf.bssid, ETH_ALEN);
+	memcpy(dot11hdr->addr2, priv->vif->addr, ETH_ALEN);
+	memcpy(dot11hdr->addr3, priv->vif->bss_conf.bssid, ETH_ALEN);
+	
+	/* Filling the LLC/SNAP Hdr */
+	snaphdr = (struct ieee80211_snap_hdr *)((u8 *)dot11hdr + framehdrlen);
+	memcpy(snaphdr, (struct ieee80211_snap_hdr *)rfc1042_header, \
+	        sizeof(*snaphdr));
+	*(u16 *)(++snaphdr) = cpu_to_be16(ETH_P_ARP);
+	/* Updating the framebdylen with snaphdr and LLC hdr size */
+	framebdylen = sizeof(*snaphdr) + 2;
+	
+	/* Filling the ARP Reply Payload */
+	arp_hdr = (struct arphdr *)((u8 *)dot11hdr + framehdrlen + framebdylen);
+	arp_hdr->ar_hrd = cpu_to_be16(ARPHRD_ETHER);
+	arp_hdr->ar_pro = cpu_to_be16(ETH_P_IP);
+	arp_hdr->ar_hln = ETH_ALEN;
+	arp_hdr->ar_pln = 4;
+	arp_hdr->ar_op = cpu_to_be16(ARPOP_REPLY);
+	
+	/* Updating the frmbdylen with Arp Reply Hdr and Arp payload size(20) */
+	framebdylen += sizeof(*arp_hdr) + 20;
+	
+	/* Updating the framebdylen with Encryption Tail Size */
+	framebdylen += encrypttailsize;
+	
+	/* Filling the Template Frame Hdr */
+	template_frame[0] = WSM_FRAME_TYPE_ARP_REPLY; /* Template frame type */
+	template_frame[1] = 0xFF; /* Rate to be fixed */
+	((u16 *)&template_frame[2])[0] = framehdrlen + framebdylen;
+	
+	ret = WARN_ON(wsm_write_mib(hw_priv, WSM_MIB_ID_TEMPLATE_FRAME, \
+	                              template_frame, (framehdrlen+framebdylen+4), 
+	                              priv->if_id));
+	kfree(template_frame);
+exit_p:
+	return ret;
+}
+
+#ifdef ROAM_OFFLOAD
+/**
+ * xradio_testmode_event -send asynchronous event
+ * to userspace
+ *
+ * @wiphy: the wiphy
+ * @msg_id: XR msg ID
+ * @data: data to be sent
+ * @len: data length
+ * @gfp: allocation flag
+ *
+ * Returns: 0 on success or non zero value on failure
+ */
+int xradio_testmode_event(struct wiphy *wiphy, const u32 msg_id,
+                          const void *data, int len, gfp_t gfp)
+{
+	struct sk_buff *skb = NULL;
+
+
+	skb = cfg80211_testmode_alloc_event_skb(wiphy, 
+	      nla_total_size(len+sizeof(msg_id)), gfp);
+
+	if (!skb)
+		return -ENOMEM;
+
+	cfg80211_testmode_event(skb, gfp);
+	return 0;
+}
+#endif /*ROAM_OFFLOAD*/
diff --git a/drivers/net/wireless/xradio/sta.h b/drivers/net/wireless/xradio/sta.h
new file mode 100644
index 0000000..42ea2f6
--- /dev/null
+++ b/drivers/net/wireless/xradio/sta.h
@@ -0,0 +1,122 @@
+/*
+ * sta interfaces for XRadio drivers
+ *
+ * Copyright (c) 2013, XRadio
+ * Author: XRadio
+ *
+ * This program is free software; you can 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 STA_H_INCLUDED
+#define STA_H_INCLUDED
+
+
+#ifdef XRADIO_USE_LONG_KEEP_ALIVE_PERIOD
+#define XRADIO_KEEP_ALIVE_PERIOD         (28)
+#else
+/*For Samsung, it is defined as 4*/
+#define XRADIO_KEEP_ALIVE_PERIOD         (4)
+#endif
+
+#ifdef XRADIO_USE_LONG_DTIM_PERIOD
+#define XRADIO_BSS_LOSS_THOLD_DEF  30
+#define XRADIO_LINK_LOSS_THOLD_DEF 50
+#else
+#define XRADIO_BSS_LOSS_THOLD_DEF  20
+#define XRADIO_LINK_LOSS_THOLD_DEF 40
+#endif
+
+/* ******************************************************************** */
+/* mac80211 API								*/
+
+int xradio_start(struct ieee80211_hw *dev);
+void xradio_stop(struct ieee80211_hw *dev);
+int xradio_add_interface(struct ieee80211_hw *dev, struct ieee80211_vif *vif);
+void xradio_remove_interface(struct ieee80211_hw *dev, struct ieee80211_vif *vif);
+int xradio_change_interface(struct ieee80211_hw *dev,
+                            struct ieee80211_vif *vif,
+                            enum nl80211_iftype new_type,
+                            bool p2p);
+int xradio_config(struct ieee80211_hw *dev, u32 changed);
+int xradio_change_interface(struct ieee80211_hw *dev,
+                            struct ieee80211_vif *vif,
+                            enum nl80211_iftype new_type,
+                            bool p2p);
+void xradio_configure_filter(struct ieee80211_hw *dev,
+                             unsigned int changed_flags,
+                             unsigned int *total_flags,
+                             u64 multicast);
+int xradio_conf_tx(struct ieee80211_hw *dev, struct ieee80211_vif *vif,
+                   u16 queue, const struct ieee80211_tx_queue_params *params);
+int xradio_get_stats(struct ieee80211_hw *dev,
+                     struct ieee80211_low_level_stats *stats);
+/* Not more a part of interface?
+int xradio_get_tx_stats(struct ieee80211_hw *dev,
+			struct ieee80211_tx_queue_stats *stats);
+*/
+int xradio_set_rts_threshold(struct ieee80211_hw *hw, u32 value);
+
+void xradio_flush(struct ieee80211_hw *hw, struct ieee80211_vif *vif, u32 queues, bool drop);
+
+
+int xradio_remain_on_channel(struct ieee80211_hw *hw,
+			     struct ieee80211_vif *vif,
+                             struct ieee80211_channel *chan,
+                             int duration, enum ieee80211_roc_type type);
+int xradio_cancel_remain_on_channel(struct ieee80211_hw *hw);
+int xradio_set_arpreply(struct ieee80211_hw *hw, struct ieee80211_vif *vif);
+u64 xradio_prepare_multicast(struct ieee80211_hw *hw,
+                             struct netdev_hw_addr_list *mc_list);
+int xradio_set_pm(struct xradio_vif *priv, const struct wsm_set_pm *arg);
+void xradio_set_data_filter(struct ieee80211_hw *hw,
+                            struct ieee80211_vif *vif,
+                            void *data,
+                            int len);
+
+/* ******************************************************************** */
+/* WSM callbacks							*/
+
+/* void xradio_set_pm_complete_cb(struct xradio_common *hw_priv,
+	struct wsm_set_pm_complete *arg); */
+void xradio_channel_switch_cb(struct xradio_common *hw_priv);
+
+/* ******************************************************************** */
+/* WSM events								*/
+
+void xradio_free_event_queue(struct xradio_common *hw_priv);
+void xradio_event_handler(struct work_struct *work);
+void xradio_bss_loss_work(struct work_struct *work);
+void xradio_connection_loss_work(struct work_struct *work);
+void xradio_keep_alive_work(struct work_struct *work);
+void xradio_tx_failure_work(struct work_struct *work);
+
+/* ******************************************************************** */
+/* Internal API								*/
+
+int xradio_setup_mac(struct xradio_common *hw_priv);
+void xradio_join_work(struct work_struct *work);
+void xradio_join_timeout(struct work_struct *work);
+void xradio_unjoin_work(struct work_struct *work);
+void xradio_offchannel_work(struct work_struct *work);
+void xradio_wep_key_work(struct work_struct *work);
+void xradio_update_filtering(struct xradio_vif *priv);
+void xradio_update_filtering_work(struct work_struct *work);
+int __xradio_flush(struct xradio_common *hw_priv, bool drop, int if_id);
+void xradio_set_beacon_wakeup_period_work(struct work_struct *work);
+int xradio_enable_listening(struct xradio_vif *priv, struct ieee80211_channel *chan);
+int xradio_disable_listening(struct xradio_vif *priv);
+int xradio_set_uapsd_param(struct xradio_vif *priv, const struct wsm_edca_params *arg);
+void xradio_ba_work(struct work_struct *work);
+void xradio_ba_timer(struct timer_list *t);
+const u8 *xradio_get_ie(u8 *start, size_t len, u8 ie);
+int xradio_vif_setup(struct xradio_vif *priv);
+int xradio_setup_mac_pvif(struct xradio_vif *priv);
+void xradio_iterate_vifs(void *data, u8 *mac, struct ieee80211_vif *vif);
+void xradio_rem_chan_timeout(struct work_struct *work);
+int xradio_set_macaddrfilter(struct xradio_common *hw_priv, struct xradio_vif *priv, u8 *data);
+#ifdef ROAM_OFFLOAD
+int xradio_testmode_event(struct wiphy *wiphy, const u32 msg_id,
+                          const void *data, int len, gfp_t gfp);
+#endif /*ROAM_OFFLOAD*/
+#endif
diff --git a/drivers/net/wireless/xradio/tx.c b/drivers/net/wireless/xradio/tx.c
new file mode 100644
index 0000000..72c04ef
--- /dev/null
+++ b/drivers/net/wireless/xradio/tx.c
@@ -0,0 +1,1455 @@
+/*
+ * Datapath implementation for XRadio drivers
+ *
+ * Copyright (c) 2013, XRadio
+ * Author: XRadio
+ *
+ * This program is free software; you can 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 <net/mac80211.h>
+#include <linux/etherdevice.h>
+#include <linux/skbuff.h>
+
+#include "xradio.h"
+#include "wsm.h"
+#include "bh.h"
+#include "ap.h"
+#include "sta.h"
+#include "sdio.h"
+#include "common.h"
+#include "p2p.h"
+
+#define B_RATE_INDEX   0     //11b rate for important short frames in 2.4G.
+#define AG_RATE_INDEX  6     //11a/g rate for important short frames in 5G.
+#define XRADIO_INVALID_RATE_ID (0xFF)
+
+/* rate should fall quickly to avoid dropping frames by aps.
+ * Add by yangfh 2014-9-22 13:39:57
+ */
+#define HIGH_RATE_MAX_RETRY  7
+
+#ifdef TES_P2P_0002_ROC_RESTART
+#include <linux/time.h>
+#endif
+
+//for test yangfh
+extern u32 tx_retrylimit;
+extern u32 tx_over_limit;
+extern u32 tx_lower_limit;
+extern int retry_mis;
+
+static const struct ieee80211_rate *
+xradio_get_tx_rate(const struct xradio_common *hw_priv,
+		   const struct ieee80211_tx_rate *rate);
+
+/* ******************************************************************** */
+/* TX policy cache implementation					*/
+
+static void tx_policy_dump(struct tx_policy *policy)
+{
+	txrx_printk(XRADIO_DBG_MSG, "[TX policy] "
+		"%.1X%.1X%.1X%.1X%.1X%.1X%.1X%.1X"
+		"%.1X%.1X%.1X%.1X%.1X%.1X%.1X%.1X"
+		"%.1X%.1X%.1X%.1X%.1X%.1X%.1X%.1X: %d\n",
+		policy->raw[0] & 0x0F,  policy->raw[0] >> 4,
+		policy->raw[1] & 0x0F,  policy->raw[1] >> 4,
+		policy->raw[2] & 0x0F,  policy->raw[2] >> 4,
+		policy->raw[3] & 0x0F,  policy->raw[3] >> 4,
+		policy->raw[4] & 0x0F,  policy->raw[4] >> 4,
+		policy->raw[5] & 0x0F,  policy->raw[5] >> 4,
+		policy->raw[6] & 0x0F,  policy->raw[6] >> 4,
+		policy->raw[7] & 0x0F,  policy->raw[7] >> 4,
+		policy->raw[8] & 0x0F,  policy->raw[8] >> 4,
+		policy->raw[9] & 0x0F,  policy->raw[9] >> 4,
+		policy->raw[10] & 0x0F,  policy->raw[10] >> 4,
+		policy->raw[11] & 0x0F,  policy->raw[11] >> 4,
+		policy->defined);
+}
+
+static void xradio_check_go_neg_conf_success(struct xradio_common *hw_priv,
+						u8 *action)
+{
+	if (action[2] == 0x50 && action[3] == 0x6F && action[4] == 0x9A &&
+		action[5] == 0x09 && action[6] == 0x02) {
+		if(action[17] == 0) {
+			hw_priv->is_go_thru_go_neg = true;
+		}
+		else {
+			hw_priv->is_go_thru_go_neg = false;
+		}
+	}
+}
+
+static void xradio_check_prov_desc_req(struct xradio_common *hw_priv,
+                                                u8 *action)
+{
+	if (action[2] == 0x50 && action[3] == 0x6F && action[4] == 0x9A &&
+	    action[5] == 0x09 && action[6] == 0x07) {
+		hw_priv->is_go_thru_go_neg = false;
+	}
+}
+
+//modified by yangfh
+static void tx_policy_build(const struct xradio_common *hw_priv,
+	/* [out] */ struct tx_policy *policy,
+	struct ieee80211_tx_rate *rates, size_t count)
+{
+	int i, j;
+	struct ieee80211_rate * tmp_rate = NULL;
+	unsigned limit = hw_priv->short_frame_max_tx_count;
+	unsigned max_rates_cnt = count;
+	unsigned total = 0;
+	BUG_ON(rates[0].idx < 0);
+	memset(policy, 0, sizeof(*policy));
+
+
+	txrx_printk(XRADIO_DBG_NIY,"============================");
+#if 0
+	//debug yangfh
+	for (i = 0; i < count; ++i) {
+		if(rates[i].idx>=0) {
+			tmp_rate = xradio_get_tx_rate(hw_priv, &rates[i]);
+			txrx_printk(XRADIO_DBG_NIY,"[TX policy] Org %d.%dMps=%d", 
+		            tmp_rate->bitrate/10, tmp_rate->bitrate%10, rates[i].count);
+		}
+	}
+	txrx_printk(XRADIO_DBG_NIY,"----------------------------");
+#endif
+	
+	/* minstrel is buggy a little bit, so distille
+	 * incoming rates first.
+	 */
+	/* Sort rates in descending order. */
+	total = rates[0].count;
+	for (i = 1; i < count; ++i) {
+		if (rates[i].idx > rates[i-1].idx) {
+			rates[i].idx = rates[i-1].idx>0?(rates[i-1].idx-1):-1;
+		}
+		if (rates[i].idx < 0 || i>=limit) {
+			count = i;
+			break;
+		} else {
+			total += rates[i].count;
+		}
+	}
+
+	/* Add lowest rate to the end when 11a/n. 
+	 * Don't apply in 11b/g because p2p unsupport 1Mbps.
+	 * TODO: it's better to do this in rate control of mac80211.
+	 */
+	if (((rates[0].flags & IEEE80211_TX_RC_MCS) || 
+		   hw_priv->channel->band == NL80211_BAND_5GHZ) && 
+		  count < max_rates_cnt && rates[count-1].idx != 0) {
+		rates[count].idx   = 0;
+		rates[count].count = rates[0].count;
+		rates[count].flags = rates[0].flags;
+		total += rates[count].count;
+		count++;
+	}
+
+	/* adjust tx count to limit, rates should fall quickly 
+	 * and lower rates should be more retry, because reorder 
+	 * buffer of reciever will be timeout and clear probably.
+	 */
+	if (count < 2) {
+		rates[0].count = limit;
+		total = limit;
+	} else {
+		u8 end_retry = 0;  //the retry should be add to last rate.
+		if (limit > HIGH_RATE_MAX_RETRY) {
+			end_retry = limit - HIGH_RATE_MAX_RETRY;
+			limit     = HIGH_RATE_MAX_RETRY;
+		}
+		for (i = 0; (limit != total) && (i < 100); ++i) {  //i<100 to avoid dead loop
+			j = i % count;
+			if(limit < total) {
+				total += (rates[j].count > 1? -1 : 0);
+				rates[j].count += (rates[j].count > 1? -1 : 0);
+			} else {
+				j = count - 1 - j;
+				if (rates[j].count > 0) {
+					total++;
+					rates[j].count++;
+				}
+			}
+		}
+		if (end_retry) {
+			rates[count-1].count += end_retry;
+			limit += end_retry;
+		}
+	}
+	
+	/* Eliminate duplicates. */
+	total = rates[0].count;
+	for (i = 0, j = 1; j < count; ++j) {
+		if (rates[j].idx == rates[i].idx) {
+			rates[i].count += rates[j].count;
+		} else if (rates[j].idx > rates[i].idx) {
+			break;
+		} else {
+			++i;
+			if (i != j)
+				rates[i] = rates[j];
+		}
+		total += rates[j].count;
+	}
+	count = i + 1;
+
+	/* Re-fill policy trying to keep every requested rate and with
+	 * respect to the global max tx retransmission count. 
+	 */
+	if (limit < count)
+		limit = count;
+	if (total > limit) {
+		for (i = 0; i < count; ++i) {
+			int left = count - i - 1;
+			if (rates[i].count > limit - left)
+				rates[i].count = limit - left;
+			limit -= rates[i].count;
+		}
+	}
+
+	/* HACK!!! Device has problems (at least) switching from
+	 * 54Mbps CTS to 1Mbps. This switch takes enormous amount
+	 * of time (100-200 ms), leading to valuable throughput drop.
+	 * As a workaround, additional g-rates are injected to the
+	 * policy.
+	 */
+	if (count == 2 && !(rates[0].flags & IEEE80211_TX_RC_MCS) &&
+			rates[0].idx > 4 && rates[0].count > 2 &&
+			rates[1].idx < 2) {
+		/* ">> 1" is an equivalent of "/ 2", but faster */
+		int mid_rate = (rates[0].idx + 4) >> 1;
+
+		/* Decrease number of retries for the initial rate */
+		rates[0].count -= 2;
+
+		if (mid_rate != 4) {
+			/* Keep fallback rate at 1Mbps. */
+			rates[3] = rates[1];
+
+			/* Inject 1 transmission on lowest g-rate */
+			rates[2].idx = 4;
+			rates[2].count = 1;
+			rates[2].flags = rates[1].flags;
+
+			/* Inject 1 transmission on mid-rate */
+			rates[1].idx = mid_rate;
+			rates[1].count = 1;
+
+			/* Fallback to 1 Mbps is a really bad thing,
+			 * so let's try to increase probability of
+			 * successful transmission on the lowest g rate
+			 * even more */
+			if (rates[0].count >= 3) {
+				--rates[0].count;
+				++rates[2].count;
+			}
+
+			/* Adjust amount of rates defined */
+			count += 2;
+		} else {
+			/* Keep fallback rate at 1Mbps. */
+			rates[2] = rates[1];
+
+			/* Inject 2 transmissions on lowest g-rate */
+			rates[1].idx = 4;
+			rates[1].count = 2;
+
+			/* Adjust amount of rates defined */
+			count += 1;
+		}
+	}
+	
+	tmp_rate = (struct ieee80211_rate *)xradio_get_tx_rate(hw_priv, &rates[0]);
+	if(tmp_rate)
+		policy->defined = tmp_rate->hw_value + 1;
+
+	for (i = 0; i < count; ++i) {
+		register unsigned rateid, off, shift, retries;
+		
+		tmp_rate = (struct ieee80211_rate *)xradio_get_tx_rate(hw_priv, &rates[i]);
+		if(tmp_rate) {
+			rateid = tmp_rate->hw_value;
+		} else {
+			break;
+		}
+		off = rateid >> 3;		/* eq. rateid / 8 */
+		shift = (rateid & 0x07) << 2;	/* eq. (rateid % 8) * 4 */
+
+		retries = rates[i].count;
+		if (unlikely(retries > 0x0F))
+			rates[i].count = retries = 0x0F;
+		policy->tbl[off] |= __cpu_to_le32(retries << shift);
+		policy->retry_count += retries;
+		txrx_printk(XRADIO_DBG_NIY,"[TX policy] %d.%dMps=%d", 
+		            tmp_rate->bitrate/10, tmp_rate->bitrate%10, retries);
+	}
+	
+	txrx_printk(XRADIO_DBG_MSG, "[TX policy] Dst Policy (%d): " \
+		"%d:%d, %d:%d, %d:%d, %d:%d, %d:%d\n",
+		count,
+		rates[0].idx, rates[0].count,
+		rates[1].idx, rates[1].count,
+		rates[2].idx, rates[2].count,
+		rates[3].idx, rates[3].count,
+		rates[4].idx, rates[4].count);
+}
+
+static inline bool tx_policy_is_equal(const struct tx_policy *wanted,
+					const struct tx_policy *cached)
+{
+	size_t count = wanted->defined >> 1;
+
+	if (wanted->defined > cached->defined)
+		return false;
+	if (count) {
+		if (memcmp(wanted->raw, cached->raw, count))
+			return false;
+	}
+	if (wanted->defined & 1) {
+		if ((wanted->raw[count] & 0x0F) != (cached->raw[count] & 0x0F))
+			return false;
+	}
+	return true;
+}
+
+static int tx_policy_find(struct tx_policy_cache *cache,
+				const struct tx_policy *wanted)
+{
+	/* O(n) complexity. Not so good, but there's only 8 entries in
+	 * the cache.
+	 * Also lru helps to reduce search time. */
+	struct tx_policy_cache_entry *it;
+	/* Search for policy in "used" list */
+	list_for_each_entry(it, &cache->used, link) {
+		if (tx_policy_is_equal(wanted, &it->policy))
+			return it - cache->cache;
+	}
+	/* Then - in "free list" */
+	list_for_each_entry(it, &cache->free, link) {
+		if (tx_policy_is_equal(wanted, &it->policy))
+			return it - cache->cache;
+	}
+	return -1;
+}
+
+static inline void tx_policy_use(struct tx_policy_cache *cache,
+				 struct tx_policy_cache_entry *entry)
+{
+	++entry->policy.usage_count;
+	list_move(&entry->link, &cache->used);
+}
+
+static inline int tx_policy_release(struct tx_policy_cache *cache,
+				    struct tx_policy_cache_entry *entry)
+{
+	int ret = --entry->policy.usage_count;
+	if (!ret)
+		list_move(&entry->link, &cache->free);
+	return ret;
+}
+
+/* ******************************************************************** */
+/* External TX policy cache API						*/
+
+void tx_policy_init(struct xradio_common *hw_priv)
+{
+	struct tx_policy_cache *cache = &hw_priv->tx_policy_cache;
+	int i;
+
+	memset(cache, 0, sizeof(*cache));
+
+	spin_lock_init(&cache->lock);
+	INIT_LIST_HEAD(&cache->used);
+	INIT_LIST_HEAD(&cache->free);
+
+	for (i = 0; i < TX_POLICY_CACHE_SIZE; ++i)
+		list_add(&cache->cache[i].link, &cache->free);
+}
+
+static int tx_policy_get(struct xradio_common *hw_priv,
+		  struct ieee80211_tx_rate *rates,
+		  u8 use_bg_rate, bool *renew)
+{
+	int idx;
+	struct tx_policy_cache *cache = &hw_priv->tx_policy_cache;
+	struct tx_policy wanted;
+
+
+	if(use_bg_rate) {
+		u8 rate  = (u8)(use_bg_rate & 0x3f);
+		u8 shitf = ((rate&0x7)<<2);
+		u8 off   = (rate>>3);
+		memset(&wanted, 0, sizeof(wanted));
+		wanted.defined = rate + 1;
+		wanted.retry_count = (hw_priv->short_frame_max_tx_count&0xf);
+		wanted.tbl[off] = wanted.retry_count<<shitf;
+		txrx_printk(XRADIO_DBG_NIY, "[TX policy] robust rate=%d\n", rate);
+	} else
+		tx_policy_build(hw_priv, &wanted, rates, IEEE80211_TX_MAX_RATES);
+
+	spin_lock_bh(&cache->lock);
+	idx = tx_policy_find(cache, &wanted);
+	if (idx >= 0) {
+		txrx_printk(XRADIO_DBG_MSG, "[TX policy] Used TX policy: %d\n",
+					idx);
+		*renew = false;
+	} else {
+		struct tx_policy_cache_entry *entry;
+		if (WARN_ON_ONCE(list_empty(&cache->free))) {
+			spin_unlock_bh(&cache->lock);
+			txrx_printk(XRADIO_DBG_ERROR, "[TX policy] no policy cache\n");
+			return XRADIO_INVALID_RATE_ID;
+		}
+		/* If policy is not found create a new one
+		 * using the oldest entry in "free" list */
+		*renew = true;
+		entry = list_entry(cache->free.prev,
+			struct tx_policy_cache_entry, link);
+		entry->policy = wanted;
+		idx = entry - cache->cache;
+		txrx_printk(XRADIO_DBG_MSG, "[TX policy] New TX policy: %d\n",
+					idx);
+		tx_policy_dump(&entry->policy);
+	}
+	tx_policy_use(cache, &cache->cache[idx]);
+	if (unlikely(list_empty(&cache->free))) {
+		/* Lock TX queues. */
+		txrx_printk(XRADIO_DBG_WARN, "[TX policy] policy cache used up\n");
+		xradio_tx_queues_lock(hw_priv);
+	}
+	spin_unlock_bh(&cache->lock);
+
+	return idx;
+}
+
+static void tx_policy_put(struct xradio_common *hw_priv, int idx)
+{
+	int usage, locked;
+	struct tx_policy_cache *cache = &hw_priv->tx_policy_cache;
+
+	spin_lock_bh(&cache->lock);
+	locked = list_empty(&cache->free);
+	usage = tx_policy_release(cache, &cache->cache[idx]);
+	if (unlikely(locked) && !usage) {
+		/* Unlock TX queues. */
+		xradio_tx_queues_unlock(hw_priv);
+	}
+	spin_unlock_bh(&cache->lock);
+}
+
+/*
+bool tx_policy_cache_full(struct xradio_common *hw_priv)
+{
+	bool ret;
+	struct tx_policy_cache *cache = &hw_priv->tx_policy_cache;
+	spin_lock_bh(&cache->lock);
+	ret = list_empty(&cache->free);
+	spin_unlock_bh(&cache->lock);
+	return ret;
+}
+*/
+extern u32 policy_upload;
+extern u32 policy_num;
+static int tx_policy_upload(struct xradio_common *hw_priv)
+{
+	struct tx_policy_cache *cache = &hw_priv->tx_policy_cache;
+	int i;
+	struct wsm_set_tx_rate_retry_policy arg = {
+		.hdr = {
+			.numTxRatePolicies = 0,
+		}
+	};
+	int if_id = 0;
+
+	spin_lock_bh(&cache->lock);
+	/* Upload only modified entries. */
+	for (i = 0; i < TX_POLICY_CACHE_SIZE; ++i) {
+		struct tx_policy *src = &cache->cache[i].policy;
+		if (src->retry_count && !src->uploaded) {
+			struct wsm_set_tx_rate_retry_policy_policy *dst =
+				&arg.tbl[arg.hdr.numTxRatePolicies];
+			dst->policyIndex = i;
+			dst->shortRetryCount = hw_priv->short_frame_max_tx_count-1;
+			//only RTS need use longRetryCount, should be short_frame.
+			dst->longRetryCount = hw_priv->short_frame_max_tx_count-1;
+			
+			/* BIT(2) - Terminate retries when Tx rate retry policy
+			 *          finishes.
+			 * BIT(3) - Count initial frame transmission as part of
+			 *          rate retry counting but not as a retry
+			 *          attempt */
+			dst->policyFlags = BIT(2) | BIT(3);
+			memcpy(dst->rateCountIndices, src->tbl,
+					sizeof(dst->rateCountIndices));
+			src->uploaded = 1;
+			++arg.hdr.numTxRatePolicies;
+		}
+	}
+	spin_unlock_bh(&cache->lock);
+	atomic_set(&hw_priv->upload_count, 0);
+	
+	txrx_printk(XRADIO_DBG_MSG, "[TX policy] Upload %d policies\n",
+				arg.hdr.numTxRatePolicies);
+
+	/*TODO: COMBO*/
+	return wsm_set_tx_rate_retry_policy(hw_priv, &arg, if_id);
+}
+
+void tx_policy_upload_work(struct work_struct *work)
+{
+	struct xradio_common *hw_priv =
+		container_of(work, struct xradio_common, tx_policy_upload_work);
+
+	WARN_ON(tx_policy_upload(hw_priv));
+	wsm_unlock_tx(hw_priv);
+}
+
+/* ******************************************************************** */
+/* xradio TX implementation						*/
+
+struct xradio_txinfo {
+	struct sk_buff *skb;
+	unsigned queue;
+	struct ieee80211_tx_info *tx_info;
+	const struct ieee80211_rate *rate;
+	struct ieee80211_hdr *hdr;
+	size_t hdrlen;
+	const u8 *da;
+	struct xradio_sta_priv *sta_priv;
+	struct xradio_txpriv txpriv;
+};
+
+u32 xradio_rate_mask_to_wsm(struct xradio_common *hw_priv, u32 rates)
+{
+	u32 ret = 0;
+	int i;
+	u32 n_bitrates = 
+	          hw_priv->hw->wiphy->bands[hw_priv->channel->band]->n_bitrates;
+	struct ieee80211_rate * bitrates = 
+	          hw_priv->hw->wiphy->bands[hw_priv->channel->band]->bitrates;
+
+	for (i = 0; i < n_bitrates; ++i) {
+		if (rates & BIT(i))
+			ret |= BIT(bitrates[i].hw_value);
+	}
+	return ret;
+}
+
+static const struct ieee80211_rate *
+xradio_get_tx_rate(const struct xradio_common *hw_priv,
+		   const struct ieee80211_tx_rate *rate)
+{
+	if (rate->idx < 0)
+		return NULL;
+	if (rate->flags & IEEE80211_TX_RC_MCS)
+		return &hw_priv->mcs_rates[rate->idx];
+	return &hw_priv->hw->wiphy->bands[hw_priv->channel->band]->
+		bitrates[rate->idx];
+}
+
+inline static s8
+xradio_get_rate_idx(const struct xradio_common *hw_priv, u8 flag, u16 hw_value)
+{
+	s16 ret = (s16)hw_value;
+	if(flag & IEEE80211_TX_RC_MCS) {  //11n
+		if(hw_value <= hw_priv->mcs_rates[7].hw_value && 
+			 hw_value >= hw_priv->mcs_rates[0].hw_value)
+			ret -= hw_priv->mcs_rates[0].hw_value;
+		else 
+			ret = -1;
+	} else {  //11b/g
+		if(hw_value>5 && hw_value<hw_priv->mcs_rates[0].hw_value) {
+			ret -= hw_priv->hw->wiphy->bands[hw_priv->channel->band]->bitrates[0].hw_value;
+			if(hw_priv->hw->wiphy->bands[hw_priv->channel->band]->bitrates[0].hw_value<5)  //11a
+				ret -= 2;
+		} else if(hw_value<4) {
+			ret -= hw_priv->hw->wiphy->bands[hw_priv->channel->band]->bitrates[0].hw_value;
+		} else {
+			ret = -1;
+		}
+	}
+	return (s8)ret;
+}
+
+static int
+xradio_tx_h_calc_link_ids(struct xradio_vif *priv,
+			  struct ieee80211_tx_control *control,
+			  struct xradio_txinfo *t)
+{
+
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+
+	if ((t->tx_info->flags & IEEE80211_TX_CTL_TX_OFFCHAN) ||
+			(hw_priv->roc_if_id == priv->if_id))
+		t->txpriv.offchannel_if_id = 2;
+	else
+		t->txpriv.offchannel_if_id = 0;
+
+	if (likely(control->sta && t->sta_priv->link_id))
+		t->txpriv.raw_link_id =
+				t->txpriv.link_id =
+				t->sta_priv->link_id;
+	else if (priv->mode != NL80211_IFTYPE_AP)
+		t->txpriv.raw_link_id =
+				t->txpriv.link_id = 0;
+	else if (is_multicast_ether_addr(t->da)) {
+		if (priv->enable_beacon) {
+			t->txpriv.raw_link_id = 0;
+			t->txpriv.link_id = priv->link_id_after_dtim;
+		} else {
+			t->txpriv.raw_link_id = 0;
+			t->txpriv.link_id = 0;
+		}
+	} else {
+		t->txpriv.link_id =
+			xradio_find_link_id(priv, t->da);
+		/* Do not assign valid link id for deauth/disassoc frame being
+		transmitted to an unassociated STA */
+		if (!(t->txpriv.link_id) &&
+			(ieee80211_is_deauth(t->hdr->frame_control) ||
+			ieee80211_is_disassoc(t->hdr->frame_control))) {
+					t->txpriv.link_id = 0;
+		} else {
+			if (!t->txpriv.link_id)
+				t->txpriv.link_id = xradio_alloc_link_id(priv, t->da);
+			if (!t->txpriv.link_id) {
+				txrx_printk(XRADIO_DBG_ERROR,
+				            "%s: No more link IDs available.\n", __func__);
+				return -ENOENT;
+			}
+		}
+		t->txpriv.raw_link_id = t->txpriv.link_id;
+	}
+	if (t->txpriv.raw_link_id)
+		priv->link_id_db[t->txpriv.raw_link_id - 1].timestamp =
+				jiffies;
+
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+	if (control->sta &&
+			(control->sta->uapsd_queues & BIT(t->queue)))
+		t->txpriv.link_id = priv->link_id_uapsd;
+#endif /* CONFIG_XRADIO_USE_EXTENSIONS */
+	return 0;
+}
+
+static void
+xradio_tx_h_pm(struct xradio_vif *priv,
+	       struct xradio_txinfo *t)
+{
+	if (unlikely(ieee80211_is_auth(t->hdr->frame_control))) {
+		u32 mask = ~BIT(t->txpriv.raw_link_id);
+		spin_lock_bh(&priv->ps_state_lock);
+		priv->sta_asleep_mask &= mask;
+		priv->pspoll_mask &= mask;
+		spin_unlock_bh(&priv->ps_state_lock);
+	}
+}
+
+static void
+xradio_tx_h_calc_tid(struct xradio_vif *priv,
+		     struct xradio_txinfo *t)
+{
+	if (ieee80211_is_data_qos(t->hdr->frame_control)) {
+		u8 *qos = ieee80211_get_qos_ctl(t->hdr);
+		t->txpriv.tid = qos[0] & IEEE80211_QOS_CTL_TID_MASK;
+	} else if (ieee80211_is_data(t->hdr->frame_control)) {
+		t->txpriv.tid = 0;
+	}
+}
+
+/* IV/ICV injection. */
+/* TODO: Quite unoptimal. It's better co modify mac80211
+ * to reserve space for IV */
+static int
+xradio_tx_h_crypt(struct xradio_vif *priv,
+		  struct xradio_txinfo *t)
+{
+	size_t iv_len;
+	size_t icv_len;
+	u8 *icv;
+
+	if (!t->tx_info->control.hw_key ||
+	    !(t->hdr->frame_control &
+	     __cpu_to_le32(IEEE80211_FCTL_PROTECTED)))
+		return 0;
+
+	iv_len = t->tx_info->control.hw_key->iv_len;
+	icv_len = t->tx_info->control.hw_key->icv_len;
+
+	if (t->tx_info->control.hw_key->cipher == WLAN_CIPHER_SUITE_TKIP)
+		icv_len += 8; /* MIC */
+
+	if (unlikely((skb_headroom(t->skb) + skb_tailroom(t->skb) <
+			 iv_len + icv_len + WSM_TX_EXTRA_HEADROOM) ||
+			(skb_headroom(t->skb) <
+			 iv_len + WSM_TX_EXTRA_HEADROOM))) {
+		dev_dbg(priv->hw_priv->pdev,
+			"no space allocated for crypto headers.\n"
+			"headroom: %d, tailroom: %d, "
+			"req_headroom: %d, req_tailroom: %d\n"
+			"Please fix it in xradio_get_skb().\n",
+			skb_headroom(t->skb), skb_tailroom(t->skb),
+			iv_len + WSM_TX_EXTRA_HEADROOM, icv_len);
+		return -ENOMEM;
+	} else if (unlikely(skb_tailroom(t->skb) < icv_len)) {
+		size_t offset = icv_len - skb_tailroom(t->skb);
+		u8 *p;
+		dev_dbg(priv->hw_priv->pdev,
+			"Slowpath: tailroom is not big enough. "
+			"Req: %d, got: %d.\n",
+			icv_len, skb_tailroom(t->skb));
+
+		p = skb_push(t->skb, offset);
+		memmove(p, &p[offset], t->skb->len - offset);
+		skb_trim(t->skb, t->skb->len - offset);
+	}
+	/* ccmp pkt from umac to driver,it has iv room,,so ccmp pkt do not add iv room */
+	if (t->tx_info->control.hw_key->cipher != WLAN_CIPHER_SUITE_CCMP){
+		u8 *newhdr;
+		newhdr = skb_push(t->skb, iv_len);
+		memmove(newhdr, newhdr + iv_len, t->hdrlen);
+		t->hdr = (struct ieee80211_hdr *) newhdr;
+	}
+	t->hdrlen += iv_len;
+	icv = skb_put(t->skb, icv_len);
+
+	return 0;
+}
+
+static int
+xradio_tx_h_align(struct xradio_vif *priv, struct xradio_txinfo *t,
+                  u8 *flags)
+{
+	size_t offset = (size_t)t->skb->data & 3;
+	u8 *newhdr;//add by dingxh
+
+
+	if (!offset)
+		return 0;
+
+	if (skb_headroom(t->skb) < offset) {
+		txrx_printk(XRADIO_DBG_ERROR,
+			"Bug: no space allocated "
+			"for DMA alignment.\n"
+			"headroom: %d\n",
+			skb_headroom(t->skb));
+		return -ENOMEM;
+	}
+    //offset = 1or3 process   add by dingxh
+	if (offset & 1) {
+		newhdr = skb_push(t->skb, offset);
+		memmove(newhdr, newhdr + offset, t->skb->len-offset);
+		skb_trim(t->skb, t->skb->len-offset);
+		t->hdr = (struct ieee80211_hdr *) newhdr;
+		return 0;
+	}
+  //add by dingxh
+	//offset=2 process
+	skb_push(t->skb, offset);
+	t->hdrlen += offset;
+	t->txpriv.offset += offset;
+	*flags |= WSM_TX_2BYTES_SHIFT;
+	return 0;
+}
+
+static int
+xradio_tx_h_action(struct xradio_vif *priv, struct xradio_txinfo *t)
+{
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)t->hdr;
+
+	if (ieee80211_is_action(t->hdr->frame_control) &&
+			mgmt->u.action.category == WLAN_CATEGORY_BACK)
+		return 1;
+	else
+		return 0;
+}
+
+/* Add WSM header */
+static struct wsm_tx *
+xradio_tx_h_wsm(struct xradio_vif *priv, struct xradio_txinfo *t)
+{
+	struct wsm_tx *wsm;
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+
+	if (skb_headroom(t->skb) < sizeof(struct wsm_tx)) {
+		txrx_printk(XRADIO_DBG_ERROR,
+			"Bug: no space allocated "
+			"for WSM header.\n"
+			"headroom: %d\n",
+			skb_headroom(t->skb));
+		return NULL;
+	}
+
+	wsm = (struct wsm_tx *)skb_push(t->skb, sizeof(struct wsm_tx));
+	t->txpriv.offset += sizeof(struct wsm_tx);
+	memset(wsm, 0, sizeof(*wsm));
+	wsm->hdr.len = __cpu_to_le16(t->skb->len);
+	wsm->hdr.id  = __cpu_to_le16(0x0004);
+	wsm->queueId = (t->txpriv.raw_link_id << 2) | wsm_queue_id_to_wsm(t->queue);
+	if (wsm->hdr.len > hw_priv->wsm_caps.sizeInpChBuf) {
+		txrx_printk(XRADIO_DBG_ERROR,"%s,msg length too big=%d\n",
+		            __func__, wsm->hdr.len);
+		wsm = NULL;
+	}
+
+	return wsm;
+}
+
+/* BT Coex specific handling */
+static void
+xradio_tx_h_bt(struct xradio_vif *priv, struct xradio_txinfo *t, struct wsm_tx *wsm)
+{
+	u8 priority = 0;
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+
+	if (!hw_priv->is_BT_Present)
+		return;
+
+	if (unlikely(ieee80211_is_nullfunc(t->hdr->frame_control)))
+		priority = WSM_EPTA_PRIORITY_MGT;
+	else if (ieee80211_is_data(t->hdr->frame_control)) {
+		/* Skip LLC SNAP header (+6) */
+		u8 *payload = &t->skb->data[t->hdrlen];
+		u16 *ethertype = (u16 *) &payload[6];
+		if (unlikely(*ethertype == __be16_to_cpu(ETH_P_PAE)))
+			priority = WSM_EPTA_PRIORITY_EAPOL;
+	} else if (unlikely(ieee80211_is_assoc_req(t->hdr->frame_control) ||
+		ieee80211_is_reassoc_req(t->hdr->frame_control))) {
+		struct ieee80211_mgmt *mgt_frame =
+				(struct ieee80211_mgmt *)t->hdr;
+
+		if (mgt_frame->u.assoc_req.listen_interval <
+						priv->listen_interval) {
+			txrx_printk(XRADIO_DBG_MSG,
+				"Modified Listen Interval to %d from %d\n",
+				priv->listen_interval,
+				mgt_frame->u.assoc_req.listen_interval);
+			/* Replace listen interval derieved from
+			 * the one read from SDD */
+			mgt_frame->u.assoc_req.listen_interval =
+				priv->listen_interval;
+		}
+	}
+
+	if (likely(!priority)) {
+		if (ieee80211_is_action(t->hdr->frame_control))
+			priority = WSM_EPTA_PRIORITY_ACTION;
+		else if (ieee80211_is_mgmt(t->hdr->frame_control))
+			priority = WSM_EPTA_PRIORITY_MGT;
+		else if ((wsm->queueId == WSM_QUEUE_VOICE))
+			priority = WSM_EPTA_PRIORITY_VOICE;
+		else if ((wsm->queueId == WSM_QUEUE_VIDEO))
+			priority = WSM_EPTA_PRIORITY_VIDEO;
+		else
+			priority = WSM_EPTA_PRIORITY_DATA;
+	}
+
+	txrx_printk(XRADIO_DBG_MSG, "[TX] EPTA priority %d.\n",
+		priority);
+
+	wsm->flags |= priority << 1;
+}
+
+static int
+xradio_tx_h_rate_policy(struct xradio_common *hw_priv, struct xradio_txinfo *t,
+                        struct wsm_tx *wsm)
+{
+	bool tx_policy_renew = false;
+	struct xradio_vif *priv =
+				xrwl_get_vif_from_ieee80211(t->tx_info->control.vif);
+
+	t->txpriv.rate_id = tx_policy_get(hw_priv,
+		t->tx_info->control.rates, t->txpriv.use_bg_rate,
+		&tx_policy_renew);
+	if (t->txpriv.rate_id == XRADIO_INVALID_RATE_ID)
+		return -EFAULT;
+
+	wsm->flags |= t->txpriv.rate_id << 4;
+	t->rate = xradio_get_tx_rate(hw_priv, &t->tx_info->control.rates[0]);
+	if (t->txpriv.use_bg_rate)
+		wsm->maxTxRate = (u8)(t->txpriv.use_bg_rate & 0x3f);
+	else
+		wsm->maxTxRate = t->rate->hw_value;
+
+	if (t->rate->flags & IEEE80211_TX_RC_MCS) {
+		if (priv->association_mode.greenfieldMode)
+			wsm->htTxParameters |=
+				__cpu_to_le32(WSM_HT_TX_GREENFIELD);
+		else
+			wsm->htTxParameters |=
+				__cpu_to_le32(WSM_HT_TX_MIXED);
+	}
+
+	if (tx_policy_renew) {
+		txrx_printk(XRADIO_DBG_MSG, "[TX] TX policy renew.\n");
+		/* It's not so optimal to stop TX queues every now and then.
+		 * Maybe it's better to reimplement task scheduling with
+		 * a counter. */
+		/* xradio_tx_queues_lock(priv); */
+		/* Definetly better. TODO. */
+		if (atomic_add_return(1, &hw_priv->upload_count) == 1) {
+			wsm_lock_tx_async(hw_priv);
+			if (queue_work(hw_priv->workqueue,
+				  &hw_priv->tx_policy_upload_work) <= 0) {
+				atomic_set(&hw_priv->upload_count, 0);
+				wsm_unlock_tx(hw_priv);
+			}
+		}
+	}
+	return 0;
+}
+
+static bool
+xradio_tx_h_pm_state(struct xradio_vif *priv, struct xradio_txinfo *t)
+{
+	int was_buffered = 1;
+
+
+	if (t->txpriv.link_id == priv->link_id_after_dtim &&
+			!priv->buffered_multicasts) {
+		priv->buffered_multicasts = true;
+		if (priv->sta_asleep_mask)
+			queue_work(priv->hw_priv->workqueue,
+				&priv->multicast_start_work);
+	}
+
+	if (t->txpriv.raw_link_id && t->txpriv.tid < XRADIO_MAX_TID)
+		was_buffered = priv->link_id_db[t->txpriv.raw_link_id - 1]
+				.buffered[t->txpriv.tid]++;
+
+	return !was_buffered;
+}
+
+static void
+xradio_tx_h_ba_stat(struct xradio_vif *priv,
+		    struct xradio_txinfo *t)
+{
+	struct xradio_common *hw_priv = priv->hw_priv;
+
+
+	if (priv->join_status != XRADIO_JOIN_STATUS_STA)
+		return;
+	if (!xradio_is_ht(&hw_priv->ht_oper))
+		return;
+	if (!priv->setbssparams_done)
+		return;
+	if (!ieee80211_is_data(t->hdr->frame_control))
+		return;
+
+	spin_lock_bh(&hw_priv->ba_lock);
+	hw_priv->ba_acc += t->skb->len - t->hdrlen;
+	if (!(hw_priv->ba_cnt_rx || hw_priv->ba_cnt)) {
+		mod_timer(&hw_priv->ba_timer,
+			jiffies + XRADIO_BLOCK_ACK_INTERVAL);
+	}
+	hw_priv->ba_cnt++;
+	spin_unlock_bh(&hw_priv->ba_lock);
+}
+
+static int
+xradio_tx_h_skb_pad(struct xradio_common *priv,
+		    struct wsm_tx *wsm,
+		    struct sk_buff *skb)
+{
+	size_t len = __le16_to_cpu(wsm->hdr.len);
+	size_t padded_len = sdio_align_len(priv, len);
+
+
+	if (WARN_ON(skb_padto(skb, padded_len) != 0)) {
+		return -EINVAL;
+	}
+	return 0;
+}
+
+void xradio_tx(struct ieee80211_hw *dev, struct ieee80211_tx_control *control, struct sk_buff *skb)
+{
+	struct xradio_common *hw_priv = dev->priv;
+	struct xradio_txinfo t = {
+		.skb = skb,
+		.queue = skb_get_queue_mapping(skb),
+		.tx_info = IEEE80211_SKB_CB(skb),
+		.hdr = (struct ieee80211_hdr *)skb->data,
+		.txpriv.tid = XRADIO_MAX_TID,
+		.txpriv.rate_id = XRADIO_INVALID_RATE_ID,
+		.txpriv.use_bg_rate = 0,
+	};
+	struct ieee80211_sta *sta;
+	struct wsm_tx *wsm;
+	bool tid_update = 0;
+	u8 flags = 0;
+	int ret = 0;
+	struct xradio_vif *priv;
+	struct ieee80211_hdr *frame = (struct ieee80211_hdr *)skb->data;
+	struct ieee80211_mgmt *mgmt = (struct ieee80211_mgmt *)skb->data;
+
+	if (!skb->data)
+		BUG_ON(1);
+
+	if (!(t.tx_info->control.vif)) {
+		ret = __LINE__;
+		goto drop;
+	}
+	priv = xrwl_get_vif_from_ieee80211(t.tx_info->control.vif);
+	if (!priv) {
+		ret = __LINE__;
+		goto drop;
+	}
+
+	if (atomic_read(&priv->enabled) == 0) {
+		ret = __LINE__;
+		goto drop;
+	}
+
+	//dhcp and 80211 frames are important, use b/g rate and delay scan.
+	//it can make sense, such as accelerate connect.
+	if (ieee80211_is_auth(frame->frame_control)) {
+		hw_priv->connet_time[priv->if_id] = jiffies;
+	} else if (ieee80211_is_data_present(frame->frame_control)) {
+		/* since Umac had already alloc IV space in ccmp skb, so we need to add this iv_len as the new offset to LLC */
+		u8* llc = NULL;
+		if(t.tx_info->control.hw_key && 
+		  	(t.hdr->frame_control & __cpu_to_le32(IEEE80211_FCTL_PROTECTED)) &&
+		  		(t.tx_info->control.hw_key->cipher == WLAN_CIPHER_SUITE_CCMP))
+			llc = skb->data+ieee80211_hdrlen(frame->frame_control) + t.tx_info->control.hw_key->iv_len;
+		else
+			llc = skb->data+ieee80211_hdrlen(frame->frame_control);
+		if (is_dhcp(llc) || is_8021x(llc)) {
+			t.txpriv.use_bg_rate = 
+			hw_priv->hw->wiphy->bands[hw_priv->channel->band]->bitrates[0].hw_value;
+			if (priv->vif->p2p)
+				t.txpriv.use_bg_rate = AG_RATE_INDEX;
+			t.txpriv.use_bg_rate |= 0x80;
+		}
+		if (t.txpriv.use_bg_rate){
+			hw_priv->connet_time[priv->if_id] = jiffies;
+		}
+	} else if (ieee80211_is_deauth(frame->frame_control) ||
+	           ieee80211_is_disassoc(frame->frame_control)) {
+		hw_priv->connet_time[priv->if_id] = 0;
+	}
+
+#ifdef AP_HT_COMPAT_FIX
+	if (ieee80211_is_assoc_req(frame->frame_control) && 
+		priv->if_id == 0 && !(priv->ht_compat_det & 0x10)) {
+		xradio_remove_ht_ie(priv, skb);
+	}
+#endif
+
+#ifdef TES_P2P_0002_ROC_RESTART
+	xradio_frame_monitor(hw_priv,skb,true);
+#endif
+
+	if (ieee80211_is_action(frame->frame_control) && 
+		mgmt->u.action.category == WLAN_CATEGORY_PUBLIC) {
+		u8 *action = (u8*)&mgmt->u.action.category;
+		xradio_check_go_neg_conf_success(hw_priv, action);
+		xradio_check_prov_desc_req(hw_priv, action);
+	}
+
+	t.txpriv.if_id = priv->if_id;
+	t.hdrlen = ieee80211_hdrlen(t.hdr->frame_control);
+	t.da = ieee80211_get_DA(t.hdr);
+	t.sta_priv =
+		(struct xradio_sta_priv *)&control->sta->drv_priv;
+
+	if (WARN_ON(t.queue >= 4)) {
+		ret = __LINE__;
+		goto drop;
+	}
+
+	//spin_lock_bh(&hw_priv->tx_queue[t.queue].lock);
+	//if ((priv->if_id == 0) &&
+	//	(hw_priv->tx_queue[t.queue].num_queued_vif[0] >=
+	//		hw_priv->vif0_throttle)) {
+	//	spin_unlock_bh(&hw_priv->tx_queue[t.queue].lock);
+	//	
+	//	ret = __LINE__;
+	//	goto drop;
+	//} else if ((priv->if_id == 1) &&
+	//	(hw_priv->tx_queue[t.queue].num_queued_vif[1] >=
+	//		hw_priv->vif1_throttle)) {
+	//	spin_unlock_bh(&hw_priv->tx_queue[t.queue].lock);
+	//	
+	//	ret = __LINE__;
+	//	goto drop;
+	//}
+	//spin_unlock_bh(&hw_priv->tx_queue[t.queue].lock);
+
+	ret = xradio_tx_h_calc_link_ids(priv, control, &t);
+	if (ret) {
+		ret = __LINE__;
+		goto drop;
+	}
+
+	dev_dbg(hw_priv->pdev, "vif %d: tx, %d bytes queue %d, link_id %d(%d).\n",
+			priv->if_id, skb->len, t.queue, t.txpriv.link_id, t.txpriv.raw_link_id);
+	if(ieee80211_is_assoc_resp(frame->frame_control)){
+		dev_dbg(hw_priv->pdev, "vif %d: association response\n", priv->if_id);
+	}
+
+	xradio_tx_h_pm(priv, &t);
+	xradio_tx_h_calc_tid(priv, &t);
+	ret = xradio_tx_h_crypt(priv, &t);
+	if (ret) {
+		ret = __LINE__;
+		goto drop;
+	}
+	ret = xradio_tx_h_align(priv, &t, &flags);
+	if (ret) {
+		ret = __LINE__;
+		goto drop;
+	}
+	ret = xradio_tx_h_action(priv, &t);
+	if (ret) {
+		ret = __LINE__;
+		goto drop;
+	}
+	wsm = xradio_tx_h_wsm(priv, &t);
+	if (!wsm) {
+		ret = __LINE__;
+		goto drop;
+	}
+
+	wsm->flags |= flags;
+	xradio_tx_h_bt(priv, &t, wsm);
+	ret = xradio_tx_h_rate_policy(hw_priv, &t, wsm);
+	if (ret) {
+		ret = __LINE__;
+		goto drop;
+	}
+
+	ret = xradio_tx_h_skb_pad(hw_priv, wsm, skb);
+	if (ret) {
+		ret = __LINE__;
+		goto drop;
+	}
+
+	rcu_read_lock();
+	sta = rcu_dereference(control->sta);
+
+	xradio_tx_h_ba_stat(priv, &t);
+	spin_lock_bh(&priv->ps_state_lock);
+	{
+		tid_update = xradio_tx_h_pm_state(priv, &t);
+		BUG_ON(xradio_queue_put(&hw_priv->tx_queue[t.queue],
+				t.skb, &t.txpriv));
+#ifdef ROC_DEBUG
+		txrx_printk(XRADIO_DBG_ERROR, "QPUT %x, %pM, if_id - %d\n",
+			t.hdr->frame_control, t.da, priv->if_id);
+#endif
+	}
+	spin_unlock_bh(&priv->ps_state_lock);
+
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+	if (tid_update && sta)
+		ieee80211_sta_set_buffered(sta,
+				t.txpriv.tid, true);
+#endif /* CONFIG_XRADIO_USE_EXTENSIONS */
+
+	rcu_read_unlock();
+
+	xradio_bh_wakeup(hw_priv);
+
+	return;
+
+drop:
+	dev_dbg(hw_priv->pdev, "dropped tx at line %d, fctl=0x%04x.\n", ret, frame->frame_control);
+	xradio_skb_dtor(hw_priv, skb, &t.txpriv);
+	return;
+}
+
+void xradio_tx_confirm_cb(struct xradio_common *hw_priv,
+			  struct wsm_tx_confirm *arg)
+{
+	u8 queue_id = xradio_queue_get_queue_id(arg->packetID);
+	struct xradio_queue *queue = &hw_priv->tx_queue[queue_id];
+	struct sk_buff *skb;
+	const struct xradio_txpriv *txpriv;
+	struct xradio_vif *priv;
+	u32    feedback_retry = 0;
+
+	priv = xrwl_hwpriv_to_vifpriv(hw_priv, arg->if_id);
+	if (unlikely(!priv))
+		return;
+
+	if (unlikely(priv->mode == NL80211_IFTYPE_UNSPECIFIED)) {
+		/* STA is stopped. */
+		spin_unlock(&priv->vif_lock);
+		return;
+	}
+
+	if (WARN_ON(queue_id >= 4)) {
+		spin_unlock(&priv->vif_lock);
+		return;
+	}
+
+	dev_dbg(hw_priv->pdev, "vif %d: tx confirm status=%d, retry=%d, lastRate=%d\n",
+			priv->if_id, arg->status, arg->ackFailures, arg->txedRate);
+
+	if ((arg->status == WSM_REQUEUE) &&
+	    (arg->flags & WSM_TX_STATUS_REQUEUE)) {
+		/* "Requeue" means "implicit suspend" */
+		struct wsm_suspend_resume suspend = {
+			.link_id = arg->link_id,
+			.stop = 1,
+			.multicast = !arg->link_id,
+			.if_id = arg->if_id,
+		};
+		xradio_suspend_resume(priv, &suspend);
+		txrx_printk(XRADIO_DBG_WARN, "Requeue for link_id %d (try %d)."
+			" STAs asleep: 0x%.8X\n",
+			arg->link_id,
+			xradio_queue_get_generation(arg->packetID) + 1,
+			priv->sta_asleep_mask);
+
+		WARN_ON(xradio_queue_requeue(queue,
+				arg->packetID, true));
+
+		spin_lock_bh(&priv->ps_state_lock);
+		if (!arg->link_id) {
+			priv->buffered_multicasts = true;
+			if (priv->sta_asleep_mask) {
+				queue_work(hw_priv->workqueue,
+					&priv->multicast_start_work);
+			}
+		}
+		spin_unlock_bh(&priv->ps_state_lock);
+		spin_unlock(&priv->vif_lock);
+	} else if (!WARN_ON(xradio_queue_get_skb(
+			queue, arg->packetID, &skb, &txpriv))) {
+		struct ieee80211_tx_info *tx = IEEE80211_SKB_CB(skb);
+		struct ieee80211_hdr *frame = (struct ieee80211_hdr *)&skb->data[txpriv->offset];
+		int tx_count = arg->ackFailures;
+		u8 ht_flags = 0;
+		int i;
+
+		//yangfh add to reset if_0 in firmware when STA-unjoined,
+		//fix the errors when switch APs in combo mode.
+		if (unlikely(ieee80211_is_disassoc(frame->frame_control) ||
+			  ieee80211_is_deauth(frame->frame_control))) {
+			if (priv->join_status == XRADIO_JOIN_STATUS_STA) {
+				wms_send_deauth_to_self(hw_priv, priv);
+				/* Shedule unjoin work */
+				txrx_printk(XRADIO_DBG_WARN, "Issue unjoin command(TX) by self.\n");
+				wsm_lock_tx_async(hw_priv);
+				if (queue_work(hw_priv->workqueue, &priv->unjoin_work) <= 0)
+					wsm_unlock_tx(hw_priv);
+			}
+		}
+
+		if (priv->association_mode.greenfieldMode)
+			ht_flags |= IEEE80211_TX_RC_GREEN_FIELD;
+
+		//bss loss confirm.
+		if (unlikely(priv->bss_loss_status == XRADIO_BSS_LOSS_CONFIRMING &&
+		    priv->bss_loss_confirm_id == arg->packetID)) {
+			spin_lock(&priv->bss_loss_lock);
+			priv->bss_loss_status = arg->status?
+			                        XRADIO_BSS_LOSS_CONFIRMED : XRADIO_BSS_LOSS_NONE;
+			spin_unlock(&priv->bss_loss_lock);
+		}
+
+		if (likely(!arg->status)) {
+			tx->flags |= IEEE80211_TX_STAT_ACK;
+			priv->cqm_tx_failure_count = 0;
+			++tx_count;
+
+			if (arg->flags & WSM_TX_STATUS_AGGREGATION) {
+				/* Do not report aggregation to mac80211:
+				 * it confuses minstrel a lot. */
+				/* tx->flags |= IEEE80211_TX_STAT_AMPDU; */
+			}
+		} else {
+			/* TODO: Update TX failure counters */
+			if (unlikely(priv->cqm_tx_failure_thold &&
+			     (++priv->cqm_tx_failure_count >
+			      priv->cqm_tx_failure_thold))) {
+				priv->cqm_tx_failure_thold = 0;
+				queue_work(hw_priv->workqueue,
+						&priv->tx_failure_work);
+			}
+			if (tx_count)
+				++tx_count;
+		}
+		spin_unlock(&priv->vif_lock);
+
+		tx->status.ampdu_len = 1;
+		tx->status.ampdu_ack_len = 1;
+
+		txrx_printk(XRADIO_DBG_NIY,"feedback:%08x, %08x, %08x.\n", 
+				         arg->rate_try[2], arg->rate_try[1], arg->rate_try[0]);
+		if(txpriv->use_bg_rate) {   //bg rates
+			tx->status.rates[0].count = arg->ackFailures+1;
+		  tx->status.rates[0].idx   = 0;
+		  tx->status.rates[1].idx   = -1;
+		  tx->status.rates[2].idx   = -1;
+		  tx->status.rates[3].idx   = -1;
+		} else {
+			int j;
+			s8  txed_idx;
+			register u8 rate_num=0, shift=0, retries=0;
+			u8  flag = tx->status.rates[0].flags;
+			
+			//get retry rate idx.
+			for(i=2; i>=0;i--) {
+				if(arg->rate_try[i]) {
+					for(j=7; j>=0;j--) {
+						shift   = j<<2;
+						retries = (arg->rate_try[i]>>shift)&0xf;
+						if(retries) {
+							feedback_retry += retries;
+							txed_idx = xradio_get_rate_idx(hw_priv,flag,((i<<3)+j));
+							txrx_printk(XRADIO_DBG_NIY, "rate_num=%d, hw=%d, idx=%d, "
+							            "retries=%d, flag=%d", rate_num, ((i<<3)+j), 
+							            txed_idx, retries, flag);
+							if(likely(txed_idx>=0)) {
+								tx->status.rates[rate_num].idx   = txed_idx;
+								tx->status.rates[rate_num].count = retries;
+								if (tx->status.rates[rate_num].flags & IEEE80211_TX_RC_MCS)
+									tx->status.rates[rate_num].flags |= ht_flags;
+								rate_num++;
+								if(rate_num>=IEEE80211_TX_MAX_RATES) {
+									i = -1;
+									break;
+								}
+							}
+						}
+					}
+				}
+			}
+			//clear other rate.
+			for (i=rate_num; i < IEEE80211_TX_MAX_RATES; ++i) {
+				tx->status.rates[i].count = 0;
+				tx->status.rates[i].idx = -1;
+			}
+			//get successful rate idx.
+			if(!arg->status) {
+				txed_idx = xradio_get_rate_idx(hw_priv, flag, arg->txedRate);
+				if(rate_num == 0) {
+					tx->status.rates[0].idx = txed_idx;
+					tx->status.rates[0].count = 1;
+				} else if(rate_num <= IEEE80211_TX_MAX_RATES){
+					--rate_num;
+					if(txed_idx == tx->status.rates[rate_num].idx) {
+						tx->status.rates[rate_num].count += 1;
+					} else if(rate_num<(IEEE80211_TX_MAX_RATES-1)){
+						++rate_num;
+						tx->status.rates[rate_num].idx   = txed_idx;
+						tx->status.rates[rate_num].count = 1;
+					} else if(txed_idx >=0) {
+						tx->status.rates[rate_num].idx   = txed_idx;
+						tx->status.rates[rate_num].count = 1;
+					}
+				}
+			} 
+		}
+
+		dev_dbg(hw_priv->pdev, "[TX policy] Ack: " \
+		"%d:%d, %d:%d, %d:%d, %d:%d\n",
+		tx->status.rates[0].idx, tx->status.rates[0].count,
+		tx->status.rates[1].idx, tx->status.rates[1].count,
+		tx->status.rates[2].idx, tx->status.rates[2].count,
+		tx->status.rates[3].idx, tx->status.rates[3].count);
+		
+
+		xradio_queue_remove(queue, arg->packetID);
+	}
+}
+
+static void xradio_notify_buffered_tx(struct xradio_vif *priv,
+			       struct sk_buff *skb, int link_id, int tid)
+{
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+	struct ieee80211_sta *sta;
+	struct ieee80211_hdr *hdr;
+	u8 *buffered;
+	u8 still_buffered = 0;
+
+
+	if (link_id && tid < XRADIO_MAX_TID) {
+		buffered = priv->link_id_db
+				[link_id - 1].buffered;
+
+		spin_lock_bh(&priv->ps_state_lock);
+		if (!WARN_ON(!buffered[tid]))
+			still_buffered = --buffered[tid];
+		spin_unlock_bh(&priv->ps_state_lock);
+
+		if (!still_buffered && tid < XRADIO_MAX_TID) {
+			hdr = (struct ieee80211_hdr *) skb->data;
+			rcu_read_lock();
+			sta = ieee80211_find_sta(priv->vif, hdr->addr1);
+			if (sta)
+				ieee80211_sta_set_buffered(sta, tid, false);
+			rcu_read_unlock();
+		}
+	}
+#endif /* CONFIG_XRADIO_USE_EXTENSIONS */
+}
+
+void xradio_skb_dtor(struct xradio_common *hw_priv,
+		     struct sk_buff *skb,
+		     const struct xradio_txpriv *txpriv)
+{
+	struct xradio_vif *priv =
+		__xrwl_hwpriv_to_vifpriv(hw_priv, txpriv->if_id);
+
+
+	skb_pull(skb, txpriv->offset);
+	if (priv && txpriv->rate_id != XRADIO_INVALID_RATE_ID) {
+		xradio_notify_buffered_tx(priv, skb,
+				txpriv->raw_link_id, txpriv->tid);
+		tx_policy_put(hw_priv, txpriv->rate_id);
+	}
+	ieee80211_tx_status(hw_priv->hw, skb);
+}
+
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+/* Workaround for WFD test case 6.1.10 */
+void xradio_link_id_reset(struct work_struct *work)
+{
+	struct xradio_vif *priv =
+		container_of(work, struct xradio_vif, linkid_reset_work);
+	struct xradio_common *hw_priv = priv->hw_priv;
+	int temp_linkid;
+
+
+	if (!priv->action_linkid) {
+		/* In GO mode we can receive ACTION frames without a linkID */
+		temp_linkid = xradio_alloc_link_id(priv,
+				&priv->action_frame_sa[0]);
+		WARN_ON(!temp_linkid);
+		if (temp_linkid) {
+			/* Make sure we execute the WQ */
+			flush_workqueue(hw_priv->workqueue);
+			/* Release the link ID */
+			spin_lock_bh(&priv->ps_state_lock);
+			priv->link_id_db[temp_linkid - 1].prev_status =
+				priv->link_id_db[temp_linkid - 1].status;
+			priv->link_id_db[temp_linkid - 1].status =
+				XRADIO_LINK_RESET;
+			spin_unlock_bh(&priv->ps_state_lock);
+			wsm_lock_tx_async(hw_priv);
+			if (queue_work(hw_priv->workqueue,
+				       &priv->link_id_work) <= 0)
+				wsm_unlock_tx(hw_priv);
+		}
+	} else {
+		spin_lock_bh(&priv->ps_state_lock);
+		priv->link_id_db[priv->action_linkid - 1].prev_status =
+			priv->link_id_db[priv->action_linkid - 1].status;
+		priv->link_id_db[priv->action_linkid - 1].status =
+			XRADIO_LINK_RESET_REMAP;
+		spin_unlock_bh(&priv->ps_state_lock);
+		wsm_lock_tx_async(hw_priv);
+		if (queue_work(hw_priv->workqueue, &priv->link_id_work) <= 0)
+				wsm_unlock_tx(hw_priv);
+		flush_workqueue(hw_priv->workqueue);
+	}
+}
+#endif
diff --git a/drivers/net/wireless/xradio/tx.h b/drivers/net/wireless/xradio/tx.h
new file mode 100644
index 0000000..e08bb71
--- /dev/null
+++ b/drivers/net/wireless/xradio/tx.h
@@ -0,0 +1,86 @@
+/*
+ * txrx interfaces for XRadio drivers
+ *
+ * Copyright (c) 2013, XRadio
+ * Author: XRadio
+ *
+ * This program is free software; you can 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 XRADIO_TXRX_H
+#define XRADIO_TXRX_H
+
+#include <linux/list.h>
+
+/* extern */ struct ieee80211_hw;
+/* extern */ struct sk_buff;
+/* extern */ struct wsm_tx;
+/* extern */ struct wsm_rx;
+/* extern */ struct wsm_tx_confirm;
+/* extern */ struct xradio_txpriv;
+/* extern */ struct xradio_vif;
+
+struct tx_policy {
+	union {
+		__le32 tbl[3];
+		u8 raw[12];
+	};
+	u8  defined;		/* TODO: u32 or u8, profile and select best */
+	u8  usage_count;	/* --// -- */
+	u8  retry_count;	/* --// -- */
+	u8  uploaded;
+};
+
+struct tx_policy_cache_entry {
+	struct tx_policy policy;
+	struct list_head link;
+};
+
+#define TX_POLICY_CACHE_SIZE	(8)
+struct tx_policy_cache {
+	struct tx_policy_cache_entry cache[TX_POLICY_CACHE_SIZE];
+	struct list_head used;
+	struct list_head free;
+	spinlock_t lock;
+};
+
+/* ******************************************************************** */
+/* TX policy cache							*/
+/* Intention of TX policy cache is an overcomplicated WSM API.
+ * Device does not accept per-PDU tx retry sequence.
+ * It uses "tx retry policy id" instead, so driver code has to sync
+ * linux tx retry sequences with a retry policy table in the device.
+ */
+void tx_policy_init(struct xradio_common *hw_priv);
+void tx_policy_upload_work(struct work_struct *work);
+
+/* ******************************************************************** */
+/* TX implementation							*/
+
+u32 xradio_rate_mask_to_wsm(struct xradio_common *hw_priv,
+			       u32 rates);
+void xradio_tx(struct ieee80211_hw *dev, struct ieee80211_tx_control *control, struct sk_buff *skb);
+void xradio_skb_dtor(struct xradio_common *hw_priv,
+		     struct sk_buff *skb,
+		     const struct xradio_txpriv *txpriv);
+
+/* ******************************************************************** */
+/* WSM callbacks							*/
+
+void xradio_tx_confirm_cb(struct xradio_common *hw_priv,
+			  struct wsm_tx_confirm *arg);
+
+/* ******************************************************************** */
+/* Timeout								*/
+
+void xradio_tx_timeout(struct work_struct *work);
+
+/* ******************************************************************** */
+/* Workaround for WFD test case 6.1.10					*/
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+void xradio_link_id_reset(struct work_struct *work);
+#endif
+
+#endif /* XRADIO_TXRX_H */
diff --git a/drivers/net/wireless/xradio/wsm.c b/drivers/net/wireless/xradio/wsm.c
new file mode 100644
index 0000000..3842116
--- /dev/null
+++ b/drivers/net/wireless/xradio/wsm.c
@@ -0,0 +1,3004 @@
+/*
+ * WSM host interfaces for XRadio drivers
+ *
+ * Copyright (c) 2013, XRadio
+ * Author: XRadio
+ *
+ * This program is free software; you can 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/skbuff.h>
+#include <linux/wait.h>
+#include <linux/skbuff.h>
+#include <linux/delay.h>
+#include <linux/sched.h>
+#include <linux/random.h>
+
+#include "xradio.h"
+#include "wsm.h"
+#include "bh.h"
+#include "ap.h"
+#include "sta.h"
+#include "rx.h"
+
+#define WSM_CMD_TIMEOUT		(2 * HZ) /* With respect to interrupt loss */
+#define WSM_CMD_JOIN_TIMEOUT	(7 * HZ) /* Join timeout is 5 sec. in FW   */
+#define WSM_CMD_START_TIMEOUT	(7 * HZ)
+#define WSM_CMD_RESET_TIMEOUT	(3 * HZ) /* 2 sec. timeout was observed.   */
+#define WSM_CMD_DEFAULT_TIMEOUT	(3 * HZ)
+#define WSM_SKIP(buf, size)						\
+	do {								\
+		if (unlikely((buf)->data + size > (buf)->end))		\
+			goto underflow;					\
+		(buf)->data += size;					\
+	} while (0)
+
+#define WSM_GET(buf, ptr, size)						\
+	do {								\
+		if (unlikely((buf)->data + size > (buf)->end))		\
+			goto underflow;					\
+		memcpy(ptr, (buf)->data, size);				\
+		(buf)->data += size;					\
+	} while (0)
+
+#define __WSM_GET(buf, type, cvt)					\
+	({								\
+		type val;						\
+		if (unlikely((buf)->data + sizeof(type) > (buf)->end))	\
+			goto underflow;					\
+		val = cvt(*(type *)(buf)->data);			\
+		(buf)->data += sizeof(type);				\
+		val;							\
+	})
+
+#define WSM_GET8(buf)  __WSM_GET(buf, u8, (u8))
+#define WSM_GET16(buf) __WSM_GET(buf, u16, __le16_to_cpu)
+#define WSM_GET32(buf) __WSM_GET(buf, u32, __le32_to_cpu)
+
+#define WSM_PUT(buf, ptr, size)						\
+	do {								\
+		if (unlikely((buf)->data + size > (buf)->end))		\
+			if (unlikely(wsm_buf_reserve((buf), size)))	\
+				goto nomem;				\
+		memcpy((buf)->data, ptr, size);				\
+		(buf)->data += size;					\
+	} while (0)
+
+#define __WSM_PUT(buf, val, type, cvt)					\
+	do {								\
+		if (unlikely((buf)->data + sizeof(type) > (buf)->end))	\
+			if (unlikely(wsm_buf_reserve((buf), sizeof(type)))) \
+				goto nomem;				\
+		*(type *)(buf)->data = cvt(val);			\
+		(buf)->data += sizeof(type);				\
+	} while (0)
+
+#define WSM_PUT8(buf, val)  __WSM_PUT(buf, val, u8, (u8))
+#define WSM_PUT16(buf, val) __WSM_PUT(buf, val, u16, __cpu_to_le16)
+#define WSM_PUT32(buf, val) __WSM_PUT(buf, val, u32, __cpu_to_le32)
+
+static void wsm_buf_reset(struct wsm_buf *buf);
+static int wsm_buf_reserve(struct wsm_buf *buf, size_t extra_size);
+static int get_interface_id_scanning(struct xradio_common *hw_priv);
+
+static int wsm_cmd_send(struct xradio_common *hw_priv,
+			struct wsm_buf *buf,
+			void *arg, u16 cmd, long tmo, int if_id);
+
+static struct xradio_vif
+	*wsm_get_interface_for_tx(struct xradio_common *hw_priv);
+
+static inline void wsm_cmd_lock(struct xradio_common *hw_priv)
+{
+	mutex_lock(&hw_priv->wsm_cmd_mux);
+}
+
+static inline void wsm_cmd_unlock(struct xradio_common *hw_priv)
+{
+	mutex_unlock(&hw_priv->wsm_cmd_mux);
+}
+
+static inline void wsm_oper_lock(struct xradio_common *hw_priv)
+{
+	mutex_lock(&hw_priv->wsm_oper_lock);
+}
+
+static inline void wsm_oper_unlock(struct xradio_common *hw_priv)
+{
+	mutex_unlock(&hw_priv->wsm_oper_lock);
+}
+
+/* ******************************************************************** */
+/* WSM API implementation						*/
+
+static int wsm_generic_confirm(struct xradio_common *hw_priv,
+			     void *arg,
+			     struct wsm_buf *buf)
+{
+	u32 status = WSM_GET32(buf);
+	if (status != WSM_STATUS_SUCCESS)
+		return -EINVAL;
+	return 0;
+
+underflow:
+	WARN_ON(1);
+	return -EINVAL;
+}
+
+#ifdef XR_RRM//RadioResourceMeasurement
+static int wsm_start_measure_requset(struct xradio_common *hw_priv,
+		                                MEASUREMENT_PARAMETERS *arg,
+		                                              int  if_id)
+{
+		int ret;
+		struct wsm_buf *buf = &hw_priv->wsm_cmd_buf;
+
+		wsm_cmd_lock(hw_priv);
+
+        WSM_PUT(buf, arg, sizeof(*arg));
+		ret = wsm_cmd_send(hw_priv, buf, arg, 0x000E, WSM_CMD_TIMEOUT, if_id);
+
+		wsm_cmd_unlock(hw_priv);
+		return ret;
+
+	nomem:
+		wsm_cmd_unlock(hw_priv);
+		return -ENOMEM;
+
+}
+
+int wsm_11k_measure_requset(struct xradio_common  *hw_priv,
+                                               u8  measure_type,
+                                              u16  ChannelNum,
+                                              u16  Duration)
+{
+    int ret;
+    u8 type, sub_type;
+    MEASUREMENT_PARAMETERS rrm_paras;
+    LMAC_MEAS_REQUEST *rrm_req = &rrm_paras.MeasurementRequest;
+//    LMAC_MEAS_CHANNEL_LOAD_PARAMS *rrm_req = &rrm_paras.MeasurementRequest;
+    rrm_paras.TxPowerLevel = 0x11;
+    rrm_paras.DurationMandatory = 0x22;
+    rrm_paras.MeasurementRequestLength = 0x33;
+    
+    type     = (measure_type&0xf0)>>4;
+    sub_type =  measure_type&0xf;
+    rrm_paras.MeasurementType = type;
+//    if (measure_type == ChannelLoadMeasurement) {
+    if (type == ChannelLoadMeasurement) {
+        rrm_req->ChannelLoadParams.Reserved = 0;
+        rrm_req->ChannelLoadParams.ChannelLoadCCA = sub_type;
+        rrm_req->ChannelLoadParams.ChannelNum = ChannelNum;
+        //valid when channelload measure, interval bettween request&start
+        rrm_req->ChannelLoadParams.RandomInterval = 0;
+        //unit:1TU=1024us
+        rrm_req->ChannelLoadParams.MeasurementDuration = Duration;
+        rrm_req->ChannelLoadParams.MeasurementStartTimel = 0;
+        rrm_req->ChannelLoadParams.MeasurementStartTimeh = 0;
+    } else if (type == NoiseHistrogramMeasurement) {
+        rrm_req->NoisHistogramParams.Reserved = 0;
+        rrm_req->NoisHistogramParams.IpiRpi = sub_type;
+        rrm_req->NoisHistogramParams.ChannelNum = ChannelNum;
+        rrm_req->NoisHistogramParams.RandomInterval = 0;
+        rrm_req->NoisHistogramParams.MeasurementDuration = Duration;
+        rrm_req->NoisHistogramParams.MeasurementStartTimel = 0;
+        rrm_req->NoisHistogramParams.MeasurementStartTimeh = 0;
+    }
+    ret = wsm_start_measure_requset(hw_priv, &rrm_paras, 0);
+    
+    return ret;
+}
+
+
+#endif//RadioResourceMeasurement
+int wsm_configuration(struct xradio_common *hw_priv,
+		      struct wsm_configuration *arg,
+		      int if_id)
+{
+	int ret;
+	struct wsm_buf *buf = &hw_priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(hw_priv);
+
+	WSM_PUT32(buf, arg->dot11MaxTransmitMsduLifeTime);
+	WSM_PUT32(buf, arg->dot11MaxReceiveLifeTime);
+	WSM_PUT32(buf, arg->dot11RtsThreshold);
+
+	/* DPD block. */
+	WSM_PUT16(buf, arg->dpdData_size + 12);
+	WSM_PUT16(buf, 1); /* DPD version */
+	WSM_PUT(buf, arg->dot11StationId, ETH_ALEN);
+	WSM_PUT16(buf, 5); /* DPD flags */
+	WSM_PUT(buf, arg->dpdData, arg->dpdData_size);
+
+	ret = wsm_cmd_send(hw_priv, buf, arg, 0x0009, WSM_CMD_TIMEOUT, if_id);
+
+	wsm_cmd_unlock(hw_priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(hw_priv);
+	return -ENOMEM;
+}
+
+static int wsm_configuration_confirm(struct xradio_common *hw_priv,
+				     struct wsm_configuration *arg,
+				     struct wsm_buf *buf)
+{
+	int i;
+	int status;
+
+	status = WSM_GET32(buf);
+	if (WARN_ON(status != WSM_STATUS_SUCCESS))
+		return -EINVAL;
+
+	WSM_GET(buf, arg->dot11StationId, ETH_ALEN);
+	arg->dot11FrequencyBandsSupported = WSM_GET8(buf);
+	WSM_SKIP(buf, 1);
+	arg->supportedRateMask = WSM_GET32(buf);
+	for (i = 0; i < 2; ++i) {
+		arg->txPowerRange[i].min_power_level = WSM_GET32(buf);
+		arg->txPowerRange[i].max_power_level = WSM_GET32(buf);
+		arg->txPowerRange[i].stepping = WSM_GET32(buf);
+	}
+	return 0;
+
+underflow:
+	WARN_ON(1);
+	return -EINVAL;
+}
+
+void wsm_query_work(struct work_struct *work)
+{
+	struct xradio_common *hw_priv =
+		container_of(work, struct xradio_common, query_work);
+	u8 ret[100] = {0};
+
+
+	*(u32*)&ret[0] = hw_priv->query_packetID;
+	wsm_read_mib(hw_priv, WSM_MIB_ID_REQ_PKT_STATUS, (void*)&ret[0], sizeof(ret), 4);
+	if(!ret[4]) {
+		wsm_printk(XRADIO_DBG_ERROR,"QuerypktID=0x%08x, status=0x%x, retry=%d, flags=0x%x, PktDebug=0x%x\n" \
+		           "pktqueue=0x%x, ext1=%d, ext2=%d, ext3=%d, ext4=0x%x, ext5=0x%x\n",
+		           *(u32*)&ret[0], ret[6], ret[7], *(u32*)&ret[8], *(u32*)&ret[12],
+		           ret[44], ret[45], ret[46], ret[47], ret[48], ret[49]);
+		wsm_printk(XRADIO_DBG_ERROR,"interdebug=0x%x, 0x%x, 0x%x, Soure=0x%x, 0x%x, 0x%x\n" \
+		           "interuse=%d, external=%d, TxOutstanding=%d, QueueStatus=0x%x, BA0=0x%x, BA1=0x%x\n" \
+		           "ScanStatus=0x%x, scanNULL=0x%x, wr_state=0x%x,0x%x,0x%x,0x%x," \
+		           "wr_cnt=%d, %d, %d, %d\n",
+		           *(u32*)&ret[16], *(u32*)&ret[20], *(u32*)&ret[24], ret[28], ret[29], ret[30],
+		           ret[32], ret[33], ret[34], ret[35], *(u32*)&ret[36], *(u32*)&ret[40],
+		           ret[50], ret[51], ret[52], ret[53], ret[54], ret[55],
+		           *(u16*)&ret[56], *(u16*)&ret[58], *(u16*)&ret[60], *(u16*)&ret[62]);
+	} else {
+		ret[5] = 0;
+		wsm_printk(XRADIO_DBG_ERROR,"No req packid=0x%08x!\n", *(u32*)&ret[0]);
+	}
+	//hardware error occurs, try to restart wifi.
+	if(ret[5] & 0x4) {
+		wsm_printk(XRADIO_DBG_ERROR,"Hardware need to reset 0x%x.\n", ret[5]);
+		hw_priv->bh_error = 1;
+		wake_up(&hw_priv->bh_wq);
+	}
+	hw_priv->query_packetID = 0;
+}
+
+/* ******************************************************************** */
+
+int wsm_reset(struct xradio_common *hw_priv, const struct wsm_reset *arg,
+		int if_id)
+{
+	int ret;
+	struct wsm_buf *buf = &hw_priv->wsm_cmd_buf;
+	u16 cmd = 0x000A | WSM_TX_LINK_ID(arg->link_id);
+
+	wsm_cmd_lock(hw_priv);
+
+	WSM_PUT32(buf, arg->reset_statistics ? 0 : 1);
+	ret = wsm_cmd_send(hw_priv, buf, NULL, cmd, WSM_CMD_RESET_TIMEOUT,
+				if_id);
+	wsm_cmd_unlock(hw_priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(hw_priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+struct wsm_mib {
+	u16 mibId;
+	void *buf;
+	size_t buf_size;
+};
+
+int wsm_read_mib(struct xradio_common *hw_priv, u16 mibId, void *_buf,
+			size_t buf_size, size_t arg_size)
+{
+	int ret;
+	struct wsm_buf *buf = &hw_priv->wsm_cmd_buf;
+	struct wsm_mib mib_buf = {
+		.mibId = mibId,
+		.buf = _buf,
+		.buf_size = buf_size,
+	};
+	wsm_cmd_lock(hw_priv);
+
+	WSM_PUT16(buf, mibId);
+	WSM_PUT16(buf, arg_size);
+	WSM_PUT(buf, _buf, arg_size);
+
+	ret = wsm_cmd_send(hw_priv, buf, &mib_buf, 0x0005, WSM_CMD_TIMEOUT, -1);
+	wsm_cmd_unlock(hw_priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(hw_priv);
+	return -ENOMEM;
+}
+
+static int wsm_read_mib_confirm(struct xradio_common *hw_priv,
+				struct wsm_mib *arg,
+				struct wsm_buf *buf)
+{
+	u16 size;
+	if (WARN_ON(WSM_GET32(buf) != WSM_STATUS_SUCCESS))
+		return -EINVAL;
+
+	if (WARN_ON(WSM_GET16(buf) != arg->mibId))
+		return -EINVAL;
+
+	size = WSM_GET16(buf);
+	if (size > arg->buf_size)
+		size = arg->buf_size;
+
+	WSM_GET(buf, arg->buf, size);
+	arg->buf_size = size;
+	return 0;
+
+underflow:
+	WARN_ON(1);
+	return -EINVAL;
+}
+
+/* ******************************************************************** */
+
+int wsm_write_mib(struct xradio_common *hw_priv, u16 mibId, void *_buf,
+			size_t buf_size, int if_id)
+{
+	int ret = 0;
+	struct wsm_buf *buf = &hw_priv->wsm_cmd_buf;
+	struct wsm_mib mib_buf = {
+		.mibId = mibId,
+		.buf = _buf,
+		.buf_size = buf_size,
+	};
+
+	wsm_cmd_lock(hw_priv);
+
+	WSM_PUT16(buf, mibId);
+	WSM_PUT16(buf, buf_size);
+	WSM_PUT(buf, _buf, buf_size);
+
+	ret = wsm_cmd_send(hw_priv, buf, &mib_buf, 0x0006, WSM_CMD_TIMEOUT,
+			if_id);
+	wsm_cmd_unlock(hw_priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(hw_priv);
+	return -ENOMEM;
+}
+
+static int wsm_write_mib_confirm(struct xradio_common *hw_priv,
+				struct wsm_mib *arg,
+				struct wsm_buf *buf,
+				int interface_link_id)
+{
+	int ret;
+	int i;
+	struct xradio_vif *priv;
+	ret = wsm_generic_confirm(hw_priv, arg, buf);
+	if (ret)
+		return ret;
+
+	/*wsm_set_operational_mode confirm.*/
+	if (arg->mibId == 0x1006) {
+		const char *p = arg->buf;
+		bool powersave_enabled = (p[0] & 0x0F) ? true : false;
+
+		/* update vif PM status. */
+		priv = xrwl_hwpriv_to_vifpriv(hw_priv, interface_link_id);
+		if (priv) {
+			xradio_enable_powersave(priv, powersave_enabled);
+			spin_unlock(&priv->vif_lock);
+		}
+
+		/* HW powersave base on vif except for generic vif. */
+		spin_lock(&hw_priv->vif_list_lock);
+		xradio_for_each_vif(hw_priv, priv, i) {
+			if (!priv)
+				continue;
+			powersave_enabled &= !!priv->powersave_enabled;
+		}
+		hw_priv->powersave_enabled = powersave_enabled;
+		spin_unlock(&hw_priv->vif_list_lock);
+
+	}
+	return 0;
+}
+
+/* ******************************************************************** */
+
+int wsm_scan(struct xradio_common *hw_priv, const struct wsm_scan *arg,
+		int if_id)
+{
+	int i;
+	int ret;
+	struct wsm_buf *buf = &hw_priv->wsm_cmd_buf;
+
+	if (unlikely(arg->numOfChannels > 48))
+		return -EINVAL;
+
+	if (unlikely(arg->numOfSSIDs > WSM_SCAN_MAX_NUM_OF_SSIDS))
+		return -EINVAL;
+
+	if (unlikely(arg->band > 1))
+		return -EINVAL;
+
+	wsm_oper_lock(hw_priv);
+	wsm_cmd_lock(hw_priv);
+
+	WSM_PUT8(buf, arg->band);
+	WSM_PUT8(buf, arg->scanType);
+	WSM_PUT8(buf, arg->scanFlags);
+	WSM_PUT8(buf, arg->maxTransmitRate);
+	WSM_PUT32(buf, arg->autoScanInterval);
+	WSM_PUT8(buf, arg->numOfProbeRequests);
+	WSM_PUT8(buf, arg->numOfChannels);
+	WSM_PUT8(buf, arg->numOfSSIDs);
+	WSM_PUT8(buf, arg->probeDelay);
+
+	for (i = 0; i < arg->numOfChannels; ++i) {
+		WSM_PUT16(buf, arg->ch[i].number);
+		WSM_PUT16(buf, 0);
+		WSM_PUT32(buf, arg->ch[i].minChannelTime);
+		WSM_PUT32(buf, arg->ch[i].maxChannelTime);
+		WSM_PUT32(buf, 0);
+	}
+
+	for (i = 0; i < arg->numOfSSIDs; ++i) {
+		WSM_PUT32(buf, arg->ssids[i].length);
+		WSM_PUT(buf, &arg->ssids[i].ssid[0],
+				sizeof(arg->ssids[i].ssid));
+	}
+
+	ret = wsm_cmd_send(hw_priv, buf, NULL, 0x0007, WSM_CMD_TIMEOUT,
+			   if_id);
+	wsm_cmd_unlock(hw_priv);
+	if (ret)
+		wsm_oper_unlock(hw_priv);
+
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(hw_priv);
+	wsm_oper_unlock(hw_priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_stop_scan(struct xradio_common *hw_priv, int if_id)
+{
+	int ret;
+	struct wsm_buf *buf = &hw_priv->wsm_cmd_buf;
+	wsm_cmd_lock(hw_priv);
+	ret = wsm_cmd_send(hw_priv, buf, NULL, 0x0008, WSM_CMD_TIMEOUT,
+			   if_id);
+	wsm_cmd_unlock(hw_priv);
+	return ret;
+}
+
+
+static int wsm_tx_confirm(struct xradio_common *hw_priv,
+			  struct wsm_buf *buf,
+			  int interface_link_id)
+{
+	struct wsm_tx_confirm tx_confirm;
+
+	tx_confirm.packetID = WSM_GET32(buf);
+	tx_confirm.status = WSM_GET32(buf);
+	tx_confirm.txedRate = WSM_GET8(buf);
+	tx_confirm.ackFailures = WSM_GET8(buf);
+	tx_confirm.flags = WSM_GET16(buf);
+	tx_confirm.rate_try[0] = WSM_GET32(buf);
+	tx_confirm.rate_try[1] = WSM_GET32(buf);
+	tx_confirm.rate_try[2] = WSM_GET32(buf);
+	tx_confirm.mediaDelay = WSM_GET32(buf);
+	tx_confirm.txQueueDelay = WSM_GET32(buf);
+
+	if (is_hardware_xradio(hw_priv)) {
+		/* TODO:COMBO:linkID will be stored in packetID*/
+		/* TODO:COMBO: Extract traffic resumption map */
+		tx_confirm.if_id = xradio_queue_get_if_id(tx_confirm.packetID);
+		tx_confirm.link_id = xradio_queue_get_link_id(
+				tx_confirm.packetID);
+	} else {
+		tx_confirm.link_id = interface_link_id;
+		tx_confirm.if_id = 0;
+	}
+
+	wsm_release_vif_tx_buffer(hw_priv, tx_confirm.if_id, 1);
+
+	xradio_tx_confirm_cb(hw_priv, &tx_confirm);
+	return 0;
+
+underflow:
+	WARN_ON(1);
+	return -EINVAL;
+}
+
+static int wsm_multi_tx_confirm(struct xradio_common *hw_priv,
+				struct wsm_buf *buf, int interface_link_id)
+{
+	struct xradio_vif *priv;
+	int ret;
+	int count;
+	int i;
+
+	count = WSM_GET32(buf);
+	if (WARN_ON(count <= 0))
+		return -EINVAL;
+	else if (count > 1) {
+		ret = wsm_release_tx_buffer(hw_priv, count - 1);
+		if (ret < 0)
+			return ret;
+		else if (ret > 0)
+			xradio_bh_wakeup(hw_priv);
+	}
+	priv = xrwl_hwpriv_to_vifpriv(hw_priv, interface_link_id);
+	if (priv) {
+		spin_unlock(&priv->vif_lock);
+	}
+	for (i = 0; i < count; ++i) {
+		ret = wsm_tx_confirm(hw_priv, buf, interface_link_id);
+		if (ret)
+			return ret;
+	}
+	return ret;
+
+underflow:
+	WARN_ON(1);
+	return -EINVAL;
+}
+
+/* ******************************************************************** */
+
+static int wsm_join_confirm(struct xradio_common *hw_priv,
+			    struct wsm_join *arg,
+			    struct wsm_buf *buf)
+{
+	if (WSM_GET32(buf) != WSM_STATUS_SUCCESS)
+		return -EINVAL;
+	arg->minPowerLevel = WSM_GET32(buf);
+	arg->maxPowerLevel = WSM_GET32(buf);
+
+	return 0;
+
+underflow:
+	WARN_ON(1);
+	return -EINVAL;
+}
+
+int wsm_join(struct xradio_common *hw_priv, struct wsm_join *arg,
+	     int if_id)
+/*TODO: combo: make it work per vif.*/
+{
+	int ret;
+	struct wsm_buf *buf = &hw_priv->wsm_cmd_buf;
+
+	wsm_oper_lock(hw_priv);
+	wsm_cmd_lock(hw_priv);
+
+	WSM_PUT8(buf, arg->mode);
+	WSM_PUT8(buf, arg->band);
+	WSM_PUT16(buf, arg->channelNumber);
+	WSM_PUT(buf, &arg->bssid[0], sizeof(arg->bssid));
+	WSM_PUT16(buf, arg->atimWindow);
+	WSM_PUT8(buf, arg->preambleType);
+	WSM_PUT8(buf, arg->probeForJoin);
+	WSM_PUT8(buf, arg->dtimPeriod);
+	WSM_PUT8(buf, arg->flags);
+	WSM_PUT32(buf, arg->ssidLength);
+	WSM_PUT(buf, &arg->ssid[0], sizeof(arg->ssid));
+	WSM_PUT32(buf, arg->beaconInterval);
+	WSM_PUT32(buf, arg->basicRateSet);
+
+	hw_priv->tx_burst_idx = -1;
+	ret = wsm_cmd_send(hw_priv, buf, arg, 0x000B, WSM_CMD_JOIN_TIMEOUT,
+			   if_id);
+	wsm_cmd_unlock(hw_priv);
+	wsm_oper_unlock(hw_priv); /*confirm, not indcation.*/
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(hw_priv);
+	wsm_oper_unlock(hw_priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_set_bss_params(struct xradio_common *hw_priv,
+			const struct wsm_set_bss_params *arg,
+			int if_id)
+{
+	int ret;
+	struct wsm_buf *buf = &hw_priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(hw_priv);
+
+	WSM_PUT8(buf, 0);
+	WSM_PUT8(buf, arg->beaconLostCount);
+	WSM_PUT16(buf, arg->aid);
+	WSM_PUT32(buf, arg->operationalRateSet);
+
+	ret = wsm_cmd_send(hw_priv, buf, NULL, 0x0011, WSM_CMD_TIMEOUT,
+			if_id);
+
+	wsm_cmd_unlock(hw_priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(hw_priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_add_key(struct xradio_common *hw_priv, const struct wsm_add_key *arg,
+			int if_id)
+{
+	int ret;
+	struct wsm_buf *buf = &hw_priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(hw_priv);
+
+	WSM_PUT(buf, arg, sizeof(*arg));
+
+	ret = wsm_cmd_send(hw_priv, buf, NULL, 0x000C, WSM_CMD_TIMEOUT,
+				if_id);
+
+	wsm_cmd_unlock(hw_priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(hw_priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_remove_key(struct xradio_common *hw_priv,
+		   const struct wsm_remove_key *arg, int if_id)
+{
+	int ret;
+	struct wsm_buf *buf = &hw_priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(hw_priv);
+
+	WSM_PUT8(buf, arg->entryIndex);
+	WSM_PUT8(buf, 0);
+	WSM_PUT16(buf, 0);
+
+	ret = wsm_cmd_send(hw_priv, buf, NULL, 0x000D, WSM_CMD_TIMEOUT,
+			   if_id);
+
+	wsm_cmd_unlock(hw_priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(hw_priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_set_tx_queue_params(struct xradio_common *hw_priv,
+				const struct wsm_set_tx_queue_params *arg,
+				u8 id, int if_id)
+{
+	int ret;
+	struct wsm_buf *buf = &hw_priv->wsm_cmd_buf;
+	u8 queue_id_to_wmm_aci[] = {3, 2, 0, 1};
+
+	wsm_cmd_lock(hw_priv);
+
+	WSM_PUT8(buf, queue_id_to_wmm_aci[id]);
+	WSM_PUT8(buf, 0);
+	WSM_PUT8(buf, arg->ackPolicy);
+	WSM_PUT8(buf, 0);
+	WSM_PUT32(buf, arg->maxTransmitLifetime);
+	WSM_PUT16(buf, arg->allowedMediumTime);
+	WSM_PUT16(buf, 0);
+
+	ret = wsm_cmd_send(hw_priv, buf, NULL, 0x0012, WSM_CMD_TIMEOUT, if_id);
+
+	wsm_cmd_unlock(hw_priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(hw_priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_set_edca_params(struct xradio_common *hw_priv,
+				const struct wsm_edca_params *arg,
+				int if_id)
+{
+	int ret;
+	struct wsm_buf *buf = &hw_priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(hw_priv);
+
+	/* Implemented according to specification. */
+
+	WSM_PUT16(buf, arg->params[3].cwMin);
+	WSM_PUT16(buf, arg->params[2].cwMin);
+	WSM_PUT16(buf, arg->params[1].cwMin);
+	WSM_PUT16(buf, arg->params[0].cwMin);
+
+	WSM_PUT16(buf, arg->params[3].cwMax);
+	WSM_PUT16(buf, arg->params[2].cwMax);
+	WSM_PUT16(buf, arg->params[1].cwMax);
+	WSM_PUT16(buf, arg->params[0].cwMax);
+
+	WSM_PUT8(buf, arg->params[3].aifns);
+	WSM_PUT8(buf, arg->params[2].aifns);
+	WSM_PUT8(buf, arg->params[1].aifns);
+	WSM_PUT8(buf, arg->params[0].aifns);
+
+	WSM_PUT16(buf, arg->params[3].txOpLimit);
+	WSM_PUT16(buf, arg->params[2].txOpLimit);
+	WSM_PUT16(buf, arg->params[1].txOpLimit);
+	WSM_PUT16(buf, arg->params[0].txOpLimit);
+
+	WSM_PUT32(buf, arg->params[3].maxReceiveLifetime);
+	WSM_PUT32(buf, arg->params[2].maxReceiveLifetime);
+	WSM_PUT32(buf, arg->params[1].maxReceiveLifetime);
+	WSM_PUT32(buf, arg->params[0].maxReceiveLifetime);
+
+	ret = wsm_cmd_send(hw_priv, buf, NULL, 0x0013, WSM_CMD_TIMEOUT, if_id);
+	wsm_cmd_unlock(hw_priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(hw_priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_switch_channel(struct xradio_common *hw_priv,
+		       const struct wsm_switch_channel *arg,
+		       int if_id)
+{
+	int ret;
+	struct wsm_buf *buf = &hw_priv->wsm_cmd_buf;
+
+	wsm_lock_tx(hw_priv);
+	wsm_cmd_lock(hw_priv);
+
+	WSM_PUT8(buf, arg->channelMode);
+	WSM_PUT8(buf, arg->channelSwitchCount);
+	WSM_PUT16(buf, arg->newChannelNumber);
+
+	hw_priv->channel_switch_in_progress = 1;
+
+	ret = wsm_cmd_send(hw_priv, buf, NULL, 0x0016, WSM_CMD_TIMEOUT, if_id);
+	wsm_cmd_unlock(hw_priv);
+	if (ret) {
+		wsm_unlock_tx(hw_priv);
+		hw_priv->channel_switch_in_progress = 0;
+	}
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(hw_priv);
+	wsm_unlock_tx(hw_priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_set_pm(struct xradio_common *hw_priv, const struct wsm_set_pm *arg,
+		int if_id)
+{
+	int ret;
+	struct wsm_buf *buf = &hw_priv->wsm_cmd_buf;
+
+	wsm_oper_lock(hw_priv);
+
+	wsm_cmd_lock(hw_priv);
+
+	WSM_PUT8(buf, arg->pmMode);
+	WSM_PUT8(buf, arg->fastPsmIdlePeriod);
+	WSM_PUT8(buf, arg->apPsmChangePeriod);
+	WSM_PUT8(buf, arg->minAutoPsPollPeriod);
+
+	ret = wsm_cmd_send(hw_priv, buf, NULL, 0x0010, WSM_CMD_TIMEOUT, if_id);
+
+	wsm_cmd_unlock(hw_priv);
+	if (ret)
+		wsm_oper_unlock(hw_priv);
+
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(hw_priv);
+	wsm_oper_unlock(hw_priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_start(struct xradio_common *hw_priv, const struct wsm_start *arg,
+		int if_id)
+{
+	int ret;
+	struct wsm_buf *buf = &hw_priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(hw_priv);
+
+	WSM_PUT8(buf, arg->mode);
+	WSM_PUT8(buf, arg->band);
+	WSM_PUT16(buf, arg->channelNumber);
+	WSM_PUT32(buf, arg->CTWindow);
+	WSM_PUT32(buf, arg->beaconInterval);
+	WSM_PUT8(buf, arg->DTIMPeriod);
+	WSM_PUT8(buf, arg->preambleType);
+	WSM_PUT8(buf, arg->probeDelay);
+	WSM_PUT8(buf, arg->ssidLength);
+	WSM_PUT(buf, arg->ssid, sizeof(arg->ssid));
+	WSM_PUT32(buf, arg->basicRateSet);
+
+	hw_priv->tx_burst_idx = -1;
+	ret = wsm_cmd_send(hw_priv, buf, NULL, 0x0017, WSM_CMD_START_TIMEOUT,
+			if_id);
+
+	wsm_cmd_unlock(hw_priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(hw_priv);
+	return -ENOMEM;
+}
+
+#if 0
+/* This API is no longer present in WSC */
+/* ******************************************************************** */
+
+int wsm_beacon_transmit(struct xradio_common *hw_priv,
+			const struct wsm_beacon_transmit *arg,
+			int if_id)
+{
+	int ret;
+	struct wsm_buf *buf = &hw_priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(hw_priv);
+
+	WSM_PUT32(buf, arg->enableBeaconing ? 1 : 0);
+
+	ret = wsm_cmd_send(hw_priv, buf, NULL, 0x0018, WSM_CMD_TIMEOUT, if_id);
+
+	wsm_cmd_unlock(hw_priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(hw_priv);
+	return -ENOMEM;
+}
+#endif
+
+/* ******************************************************************** */
+
+int wsm_start_find(struct xradio_common *hw_priv, int if_id)
+{
+	int ret;
+	struct wsm_buf *buf = &hw_priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(hw_priv);
+	ret = wsm_cmd_send(hw_priv, buf, NULL, 0x0019, WSM_CMD_TIMEOUT, if_id);
+	wsm_cmd_unlock(hw_priv);
+	return ret;
+}
+
+/* ******************************************************************** */
+
+int wsm_stop_find(struct xradio_common *hw_priv, int if_id)
+{
+	int ret;
+	struct wsm_buf *buf = &hw_priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(hw_priv);
+	ret = wsm_cmd_send(hw_priv, buf, NULL, 0x001A, WSM_CMD_TIMEOUT, if_id);
+	wsm_cmd_unlock(hw_priv);
+	return ret;
+}
+
+/* ******************************************************************** */
+
+int wsm_map_link(struct xradio_common *hw_priv, const struct wsm_map_link *arg,
+		int if_id)
+{
+	int ret;
+	struct wsm_buf *buf = &hw_priv->wsm_cmd_buf;
+	u16 cmd = 0x001C;
+
+	wsm_cmd_lock(hw_priv);
+
+	WSM_PUT(buf, &arg->mac_addr[0], sizeof(arg->mac_addr));
+
+	if (is_hardware_xradio(hw_priv)) {
+		WSM_PUT8(buf, arg->unmap);
+		WSM_PUT8(buf, arg->link_id);
+	} else {
+		cmd |= WSM_TX_LINK_ID(arg->link_id);
+		WSM_PUT16(buf, 0);
+	}
+
+	ret = wsm_cmd_send(hw_priv, buf, NULL, cmd, WSM_CMD_TIMEOUT, if_id);
+
+	wsm_cmd_unlock(hw_priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(hw_priv);
+	return -ENOMEM;
+}
+
+/* ******************************************************************** */
+
+int wsm_update_ie(struct xradio_common *hw_priv,
+		  const struct wsm_update_ie *arg, int if_id)
+{
+	int ret;
+	struct wsm_buf *buf = &hw_priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(hw_priv);
+
+	WSM_PUT16(buf, arg->what);
+	WSM_PUT16(buf, arg->count);
+	WSM_PUT(buf, arg->ies, arg->length);
+
+	ret = wsm_cmd_send(hw_priv, buf, NULL, 0x001B, WSM_CMD_TIMEOUT, if_id);
+
+	wsm_cmd_unlock(hw_priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(hw_priv);
+	return -ENOMEM;
+
+}
+/* ******************************************************************** */
+#ifdef MCAST_FWDING
+/* 3.66 */
+static int wsm_give_buffer_confirm(struct xradio_common *hw_priv,
+                            struct wsm_buf *buf)
+{
+	wsm_printk(XRADIO_DBG_MSG, "HW Buf count %d\n", hw_priv->hw_bufs_used);
+	if (!hw_priv->hw_bufs_used)
+		wake_up(&hw_priv->bh_evt_wq);
+
+	return 0;
+}
+
+/* 3.65 */
+int wsm_init_release_buffer_request(struct xradio_common *hw_priv, u8 index)
+{
+	struct wsm_buf *buf = &hw_priv->wsm_release_buf[index];
+	u16 cmd = 0x0022; /* Buffer Request */
+	u8 flags;
+	size_t buf_len;
+
+	wsm_buf_init(buf);
+
+	flags = index ? 0: 0x1;
+
+	WSM_PUT8(buf, flags);
+	WSM_PUT8(buf, 0);
+	WSM_PUT16(buf, 0);
+
+	buf_len = buf->data - buf->begin;
+
+	/* Fill HI message header */
+	((__le16 *)buf->begin)[0] = __cpu_to_le16(buf_len);
+	((__le16 *)buf->begin)[1] = __cpu_to_le16(cmd);
+
+	return 0;
+nomem:
+	return -ENOMEM;
+}
+
+/* 3.65 fixed memory leakage by yangfh*/
+int wsm_deinit_release_buffer(struct xradio_common *hw_priv)
+{
+	struct wsm_buf *buf = NULL;
+	int i, err = 0;
+	
+	for (i = 0; i < WSM_MAX_BUF; i++) {
+		buf = &hw_priv->wsm_release_buf[i];
+		if(likely(buf)) {
+			if(likely(buf->begin))
+				kfree(buf->begin);
+			buf->begin = buf->data = buf->end = NULL;
+		} else {
+			err++;
+		}
+	}
+	if(err) wsm_printk(XRADIO_DBG_ERROR, "%s, NULL buf=%d!\n", __func__, err);
+	return 0;
+}
+
+/* 3.68 */
+static int wsm_request_buffer_confirm(struct xradio_vif *priv,
+                            u8 *arg,
+                            struct wsm_buf *buf)
+{
+	u8 count;
+	u32 sta_asleep_mask = 0;
+	int i;
+	u32 mask = 0;
+	u32 change_mask = 0;
+	struct xradio_common *hw_priv = priv->hw_priv;
+
+	/* There is no status field in this message */
+	sta_asleep_mask = WSM_GET32(buf);
+	count = WSM_GET8(buf);
+	count -= 1; /* Current workaround for FW issue */
+
+	spin_lock_bh(&priv->ps_state_lock);
+	change_mask = (priv->sta_asleep_mask ^ sta_asleep_mask);
+	wsm_printk(XRADIO_DBG_MSG, "CM %x, HM %x, FWM %x\n", change_mask,priv->sta_asleep_mask, sta_asleep_mask);
+	spin_unlock_bh(&priv->ps_state_lock);
+
+	if (change_mask) {
+		struct ieee80211_sta *sta;
+		int ret = 0;
+
+
+		for (i = 0; i < MAX_STA_IN_AP_MODE ; ++i) {
+
+			if(XRADIO_LINK_HARD != priv->link_id_db[i].status)
+				continue;
+
+			mask = BIT(i + 1);
+
+			/* If FW state and host state for this link are different then notify OMAC */
+			if(change_mask & mask) {
+				wsm_printk(XRADIO_DBG_MSG, "PS State Changed %d for sta %pM\n", (sta_asleep_mask & mask) ? 1:0, priv->link_id_db[i].mac);
+				rcu_read_lock();
+				sta = ieee80211_find_sta(priv->vif, priv->link_id_db[i].mac);
+				if (!sta) {
+					wsm_printk(XRADIO_DBG_MSG, "WRBC - could not find sta %pM\n",
+							priv->link_id_db[i].mac);
+				} else {
+					ret = ieee80211_sta_ps_transition_ni(sta, (sta_asleep_mask & mask) ? true: false);
+					wsm_printk(XRADIO_DBG_MSG, "PS State NOTIFIED %d\n", ret);
+					WARN_ON(ret);
+				}
+				rcu_read_unlock();			
+			}
+		}
+		/* Replace STA mask with one reported by FW */
+		spin_lock_bh(&priv->ps_state_lock);
+		priv->sta_asleep_mask = sta_asleep_mask;
+		spin_unlock_bh(&priv->ps_state_lock);
+	}
+
+	wsm_printk(XRADIO_DBG_MSG, "WRBC - HW Buf count %d SleepMask %d\n",
+					hw_priv->hw_bufs_used, sta_asleep_mask);
+	hw_priv->buf_released = 0;
+	WARN_ON(count != (hw_priv->wsm_caps.numInpChBufs - 1));
+
+    return 0;
+
+underflow:
+    WARN_ON(1);
+    return -EINVAL;
+}
+
+/* 3.67 */
+int wsm_request_buffer_request(struct xradio_vif *priv,
+				u8 *arg)
+{
+	int ret;
+	struct wsm_buf *buf = &priv->hw_priv->wsm_cmd_buf;
+
+	wsm_cmd_lock(priv->hw_priv);
+
+	WSM_PUT8(buf, (*arg));
+	WSM_PUT8(buf, 0);
+	WSM_PUT16(buf, 0);
+
+	ret = wsm_cmd_send(priv->hw_priv, buf, arg, 0x0023, WSM_CMD_JOIN_TIMEOUT,priv->if_id);
+
+	wsm_cmd_unlock(priv->hw_priv);
+	return ret;
+
+nomem:
+	wsm_cmd_unlock(priv->hw_priv);
+	return -ENOMEM;
+}
+
+#endif
+
+int wsm_set_keepalive_filter(struct xradio_vif *priv, bool enable)
+{
+        struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+
+        priv->rx_filter.keepalive = enable;
+        return wsm_set_rx_filter(hw_priv, &priv->rx_filter, priv->if_id);
+}
+
+int wsm_set_probe_responder(struct xradio_vif *priv, bool enable)
+{
+        struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+
+        priv->rx_filter.probeResponder = enable;
+        return wsm_set_rx_filter(hw_priv, &priv->rx_filter, priv->if_id);
+}
+/* ******************************************************************** */
+/* WSM indication events implementation					*/
+
+static int wsm_startup_indication(struct xradio_common *hw_priv,
+					struct wsm_buf *buf)
+{
+	u16 status;
+	static const char * const fw_types[] = {
+			"ETF",
+			"WFM",
+			"WSM",
+			"HI test",
+			"Platform test"
+	};
+
+	hw_priv->wsm_caps.numInpChBufs	= WSM_GET16(buf);
+	hw_priv->wsm_caps.sizeInpChBuf	= WSM_GET16(buf);
+	hw_priv->wsm_caps.hardwareId	= WSM_GET16(buf);
+	hw_priv->wsm_caps.hardwareSubId	= WSM_GET16(buf);
+	status				= WSM_GET16(buf);
+	hw_priv->wsm_caps.firmwareCap	= WSM_GET16(buf);
+	hw_priv->wsm_caps.firmwareType	= WSM_GET16(buf);
+	hw_priv->wsm_caps.firmwareApiVer	= WSM_GET16(buf);
+	hw_priv->wsm_caps.firmwareBuildNumber = WSM_GET16(buf);
+	hw_priv->wsm_caps.firmwareVersion	= WSM_GET16(buf);
+	WSM_GET(buf, &hw_priv->wsm_caps.fw_label[0], WSM_FW_LABEL);
+	hw_priv->wsm_caps.fw_label[WSM_FW_LABEL+1] = 0; /* Do not trust FW too much. */
+
+	if (WARN_ON(status))
+		return -EINVAL;
+
+	if (WARN_ON(hw_priv->wsm_caps.firmwareType > 4))
+		return -EINVAL;
+
+	dev_info(hw_priv->pdev,
+		"   Input buffers: %d x %d bytes\n"
+		"   Hardware: %d.%d\n"
+		"   %s firmware ver: %d, build: %d,"
+		    " api: %d, cap: 0x%.4X\n",
+		hw_priv->wsm_caps.numInpChBufs,
+		hw_priv->wsm_caps.sizeInpChBuf,
+		hw_priv->wsm_caps.hardwareId,
+		hw_priv->wsm_caps.hardwareSubId,
+		fw_types[hw_priv->wsm_caps.firmwareType],
+		hw_priv->wsm_caps.firmwareVersion,
+		hw_priv->wsm_caps.firmwareBuildNumber,
+		hw_priv->wsm_caps.firmwareApiVer,
+		hw_priv->wsm_caps.firmwareCap);
+	
+	dev_info(hw_priv->pdev, "Firmware Label:%s\n", &hw_priv->wsm_caps.fw_label[0]);
+
+	hw_priv->wsm_caps.firmwareReady = 1;
+
+	wake_up(&hw_priv->wsm_startup_done);
+	return 0;
+
+underflow:
+	WARN_ON(1);
+	return -EINVAL;
+}
+
+//add by yangfh 2014-10-31 16:58:53
+void wms_send_deauth_to_self(struct xradio_common *hw_priv, struct xradio_vif *priv)
+{
+	struct sk_buff *skb = NULL;
+	struct ieee80211_mgmt *deauth = NULL;
+
+	if (priv->join_status == XRADIO_JOIN_STATUS_AP) {
+		int i = 0;
+		wsm_printk(XRADIO_DBG_WARN, "AP mode, send_deauth_to_self\n");
+		for (i = 0; i<MAX_STA_IN_AP_MODE; i++) {
+			if (priv->link_id_db[i].status == XRADIO_LINK_HARD) {
+				skb = dev_alloc_skb(sizeof(struct ieee80211_mgmt) + 64);
+				if (!skb)
+					return;
+				skb_reserve(skb, 64);
+				deauth = (struct ieee80211_mgmt *)skb_put(skb, sizeof(struct ieee80211_mgmt));
+				if(!deauth) {
+					WARN_ON(1);
+					return;
+				}
+				deauth->frame_control =
+				    cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_DEAUTH);
+				deauth->duration = 0;
+				memcpy(deauth->da, priv->vif->addr, ETH_ALEN);
+				memcpy(deauth->sa, priv->link_id_db[i].mac, ETH_ALEN);
+				memcpy(deauth->bssid, priv->vif->addr, ETH_ALEN);
+				deauth->seq_ctrl = 0;
+				deauth->u.deauth.reason_code = WLAN_REASON_DEAUTH_LEAVING;
+				ieee80211_rx_irqsafe(priv->hw, skb);
+			}
+		}
+	} else if (priv->join_status == XRADIO_JOIN_STATUS_STA) {
+		wsm_printk(XRADIO_DBG_WARN, "STA mode, send_deauth_to_self\n");
+		skb = dev_alloc_skb(sizeof(struct ieee80211_mgmt) + 64);
+		if (!skb)
+			return;
+		skb_reserve(skb, 64);
+		deauth = (struct ieee80211_mgmt *)skb_put(skb, sizeof(struct ieee80211_mgmt));
+		if(!deauth) {
+			WARN_ON(1);
+			return;
+		}
+		deauth->frame_control =
+		    cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_DEAUTH);
+		deauth->duration = 0;
+		memcpy(deauth->da, priv->vif->addr, ETH_ALEN);
+		memcpy(deauth->sa, priv->join_bssid, ETH_ALEN);
+		memcpy(deauth->bssid, priv->join_bssid, ETH_ALEN);
+		deauth->seq_ctrl = 0;
+		deauth->u.deauth.reason_code = WLAN_REASON_DEAUTH_LEAVING;
+		ieee80211_rx_irqsafe(priv->hw, skb);
+	}
+}
+
+void wms_send_disassoc_to_self(struct xradio_common *hw_priv, struct xradio_vif *priv)
+{
+	struct sk_buff *skb = NULL;
+	struct ieee80211_mgmt *disassoc = NULL;
+	if (priv->join_status == XRADIO_JOIN_STATUS_AP) {
+		int i = 0;
+		wsm_printk(XRADIO_DBG_WARN, "AP mode, wms_send_disassoc_to_self\n");
+		for (i = 0; i<MAX_STA_IN_AP_MODE; i++) {
+			if (priv->link_id_db[i].status == XRADIO_LINK_HARD) {
+				skb = dev_alloc_skb(sizeof(struct ieee80211_mgmt) + 64);
+				if (!skb)
+					return;
+				skb_reserve(skb, 64);
+				disassoc = (struct ieee80211_mgmt *)skb_put(skb, sizeof(struct ieee80211_mgmt));
+				if(!disassoc) {
+					WARN_ON(1);
+					return;
+				}
+				disassoc->frame_control =
+					cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_DISASSOC);
+				disassoc->duration = 0;
+				memcpy(disassoc->da, priv->vif->addr, ETH_ALEN);
+				memcpy(disassoc->sa, priv->link_id_db[i].mac, ETH_ALEN);
+				memcpy(disassoc->bssid, priv->vif->addr, ETH_ALEN);
+				disassoc->seq_ctrl = 0;
+				disassoc->u.disassoc.reason_code = WLAN_REASON_DISASSOC_STA_HAS_LEFT;
+				ieee80211_rx_irqsafe(priv->hw, skb);
+			}
+		}
+	} else if (priv->join_status == XRADIO_JOIN_STATUS_STA) {
+		wsm_printk(XRADIO_DBG_WARN, "STA mode, wms_send_disassoc_to_self\n");
+		skb = dev_alloc_skb(sizeof(struct ieee80211_mgmt) + 64);
+		if (!skb)
+			return;
+		skb_reserve(skb, 64);
+		disassoc = (struct ieee80211_mgmt *)skb_put(skb, sizeof(struct ieee80211_mgmt));
+		if(!disassoc) {
+			WARN_ON(1);
+			return;
+		}
+		disassoc->frame_control =
+		     cpu_to_le16(IEEE80211_FTYPE_MGMT | IEEE80211_STYPE_DISASSOC);
+		disassoc->duration = 0;
+		memcpy(disassoc->da, priv->vif->addr, ETH_ALEN);
+		memcpy(disassoc->sa, priv->join_bssid, ETH_ALEN);
+		memcpy(disassoc->bssid, priv->join_bssid, ETH_ALEN);
+		disassoc->seq_ctrl = 0;
+		disassoc->u.disassoc.reason_code = WLAN_REASON_DISASSOC_DUE_TO_INACTIVITY;
+		ieee80211_rx_irqsafe(priv->hw, skb);
+	}
+}
+
+static int wsm_receive_indication(struct xradio_common *hw_priv,
+					int interface_link_id,
+					struct wsm_buf *buf,
+					struct sk_buff **skb_p)
+{
+	struct xradio_vif *priv;
+	struct wsm_rx rx;
+	struct ieee80211_hdr *hdr;
+	size_t hdr_len;
+
+	hw_priv->rx_timestamp = jiffies;
+
+		rx.status = WSM_GET32(buf);
+		rx.channelNumber = WSM_GET16(buf);
+		rx.rxedRate = WSM_GET8(buf);
+		rx.rcpiRssi = WSM_GET8(buf);
+		rx.flags = WSM_GET32(buf);
+
+		/* TODO:COMBO: Frames received from scanning are received
+		* with interface ID == 2 */
+		if (is_hardware_xradio(hw_priv)) {
+			if (interface_link_id == XRWL_GENERIC_IF_ID) {
+				/* Frames received in response to SCAN
+				 * Request */
+				interface_link_id =
+					get_interface_id_scanning(hw_priv);
+				if (interface_link_id == -1) {
+					interface_link_id = hw_priv->roc_if_id;
+				}
+#ifdef ROAM_OFFLOAD
+				if (hw_priv->auto_scanning) {
+					interface_link_id = hw_priv->scan.if_id;
+				}
+#endif/*ROAM_OFFLOAD*/
+			}
+			/* linkid (peer sta id is encoded in bit 25-28 of
+			   flags field */
+			rx.link_id = ((rx.flags & (0xf << 25)) >> 25);
+			rx.if_id = interface_link_id;
+		} else {
+			rx.link_id = interface_link_id;
+			rx.if_id = 0;
+		}
+		priv = xrwl_hwpriv_to_vifpriv(hw_priv, rx.if_id);
+		if (!priv) {
+			dev_dbg(hw_priv->pdev, "got frame on a vif we don't have, dropped\n");
+			return 0;
+		}
+		//remove wsm hdr of skb
+		hdr_len = buf->data - buf->begin;
+		skb_pull(*skb_p, hdr_len);
+		
+		/* FW Workaround: Drop probe resp or
+		beacon when RSSI is 0 */
+		hdr = (struct ieee80211_hdr *) (*skb_p)->data;
+
+		if (!rx.rcpiRssi &&
+		    (ieee80211_is_probe_resp(hdr->frame_control) ||
+		    ieee80211_is_beacon(hdr->frame_control))) {
+			spin_unlock(&priv->vif_lock);
+			return 0;
+		}
+
+		/* If no RSSI subscription has been made,
+		* convert RCPI to RSSI here */
+		if (!priv->cqm_use_rssi)
+			rx.rcpiRssi = rx.rcpiRssi / 2 - 110;
+
+		if (!rx.status && unlikely(ieee80211_is_deauth(hdr->frame_control))) {
+			if (priv->join_status == XRADIO_JOIN_STATUS_STA) {
+				/* Shedule unjoin work */
+				dev_dbg(hw_priv->pdev,
+					"Issue unjoin command (RX).\n");
+				wsm_lock_tx_async(hw_priv);
+				if (queue_work(hw_priv->workqueue,
+						&priv->unjoin_work) <= 0)
+					wsm_unlock_tx(hw_priv);
+			}
+		}
+		xradio_rx_cb(priv, &rx, skb_p);
+		if (*skb_p)
+			skb_push(*skb_p, hdr_len);
+		spin_unlock(&priv->vif_lock);
+
+	return 0;
+
+underflow:
+	return -EINVAL;
+}
+
+static int wsm_event_indication(struct xradio_common *hw_priv,
+				struct wsm_buf *buf,
+				int interface_link_id)
+{
+	int first;
+	struct xradio_wsm_event *event = NULL;
+	struct xradio_vif *priv;
+
+	if (!is_hardware_xradio(hw_priv))
+		interface_link_id = 0;
+
+	priv = xrwl_hwpriv_to_vifpriv(hw_priv, interface_link_id);
+
+	if (unlikely(!priv)) {
+		dev_warn(hw_priv->pdev, "Event: %d(%d) for removed "
+			   "interface, ignoring\n", __le32_to_cpu(WSM_GET32(buf)),
+			   __le32_to_cpu(WSM_GET32(buf)));
+		return 0;
+	}
+
+	if (unlikely(priv->mode == NL80211_IFTYPE_UNSPECIFIED)) {
+		/* STA is stopped. */
+		return 0;
+	}
+	spin_unlock(&priv->vif_lock);
+
+	event = kzalloc(sizeof(struct xradio_wsm_event), GFP_KERNEL);
+	if (event == NULL) {
+		dev_err(hw_priv->pdev, "xr_kzalloc failed!");
+		return -EINVAL;
+	}
+
+	event->evt.eventId = __le32_to_cpu(WSM_GET32(buf));
+	event->evt.eventData = __le32_to_cpu(WSM_GET32(buf));
+	event->if_id = interface_link_id;
+
+	dev_dbg(hw_priv->pdev, "Event: %d(%d)\n",
+		event->evt.eventId, event->evt.eventData);
+
+	spin_lock(&hw_priv->event_queue_lock);
+	first = list_empty(&hw_priv->event_queue);
+	list_add_tail(&event->link, &hw_priv->event_queue);
+	spin_unlock(&hw_priv->event_queue_lock);
+
+	if (first)
+		queue_work(hw_priv->workqueue, &hw_priv->event_handler);
+
+	return 0;
+
+underflow:
+	kfree(event);
+	return -EINVAL;
+}
+
+#define PRINT_11K_MEASRURE 1
+static int wsm_measure_cmpl_indication(struct xradio_common *hw_priv,
+				                       struct wsm_buf *buf)
+{
+    MEASUREMENT_COMPLETE measure_cmpl;
+    u8 cca_chanload;
+    u32 buf_len = 0;
+    u32 *data;
+	
+    LMAC_MEAS_CHANNEL_LOAD_RESULTS *chanload_res;
+    LMAC_MEAS_NOISE_HISTOGRAM_RESULTS *noise_res;
+	WSM_GET(buf, &measure_cmpl, 12);
+
+    switch (measure_cmpl.MeasurementType) {
+	    case ChannelLoadMeasurement:
+	        buf_len = sizeof(LMAC_MEAS_CHANNEL_LOAD_RESULTS);
+	        break;
+	    case NoiseHistrogramMeasurement:
+	        buf_len = sizeof(LMAC_MEAS_NOISE_HISTOGRAM_RESULTS);
+	        break;
+	    case BeaconReport:
+	        buf_len = sizeof(LMAC_MEAS_BEACON_RESULTS);
+	        break;
+	    case STAstatisticsReport:
+	        buf_len = sizeof(LMAC_MEAS_STA_STATS_RESULTS);
+	        break;
+	    case LinkMeasurement:
+	        buf_len = sizeof(LMAC_MEAS_LINK_MEASUREMENT_RESULTS);
+	        break;
+	}
+	wsm_printk(XRADIO_DBG_ERROR, "[11K]buf_len = %d\n", buf_len);
+    WSM_GET(buf, &measure_cmpl.MeasurementReport, buf_len);
+	
+	data = (u32 *)(&measure_cmpl);
+//	wsm_printk(XRADIO_DBG_ERROR, "[***HL***]data[0]=%08x\n", data[0]);
+//	wsm_printk(XRADIO_DBG_ERROR, "[***HL***]data[1]=%08x\n", data[1]);
+//	wsm_printk(XRADIO_DBG_ERROR, "[***HL***]data[2]=%08x\n", data[2]);
+//	wsm_printk(XRADIO_DBG_ERROR, "[***HL***]data[3]=%08x\n", data[3]);
+//	wsm_printk(XRADIO_DBG_ERROR, "[***HL***]data[4]=%08x\n", data[4]);
+//	wsm_printk(XRADIO_DBG_ERROR, "[***HL***]data[5]=%08x\n", data[5]);
+//	wsm_printk(XRADIO_DBG_ERROR, "[***HL***]data[6]=%08x\n", data[6]);
+	wsm_printk(XRADIO_DBG_ERROR, "[***HL***]MeasurementType=%0d\n", measure_cmpl.MeasurementType);
+	
+	if (measure_cmpl.Status == WSM_STATUS_SUCCESS){
+	    switch (measure_cmpl.MeasurementType) {
+	        case ChannelLoadMeasurement:
+	            chanload_res = &measure_cmpl.MeasurementReport.ChannelLoadResults;
+	            cca_chanload = (chanload_res->ChannelLoadCCA == MEAS_CCA) ? 
+	                            chanload_res->CCAbusyFraction : 
+	                            chanload_res->ChannelLoad;
+	            #ifdef PRINT_11K_MEASRURE
+	                wsm_printk(XRADIO_DBG_ERROR, "[11K] ChannelLoadMeasurement Result:\n"\
+	                                             "ChannelLoadCCA = %d\n"\
+	                                             "ChannelNum     = %d\n"\
+	                                             "Duration       = %d\n"\
+	                                             "Fraction       = %d\n", \
+            	               chanload_res->ChannelLoadCCA,\
+            	               chanload_res->ChannelNum,\
+            	               chanload_res->MeasurementDuration,\
+            	               cca_chanload
+            	               );
+	            #endif
+	            break;
+	        case NoiseHistrogramMeasurement:
+	            noise_res = &measure_cmpl.MeasurementReport.NoiseHistogramResults;
+//	            IpiRpi = (noise_res->IpiRpi == MEAS_RPI) ? 
+//	                            chanload_res->CCAbusyFraction : 
+//	                            chanload_res->ChannelLoad;
+	            #ifdef PRINT_11K_MEASRURE
+	                wsm_printk(XRADIO_DBG_ERROR, "[11K] NoiseHistogramResults:\n"\
+	                                             "IpiRpi = %d\n"\
+	                                             "ChannelNum = %d\n"\
+	                                             "PI_0__Density = %d\n"\
+	                                             "PI_1__Density = %d\n"\
+	                                             "PI_2__Density = %d\n"\
+	                                             "PI_3__Density = %d\n"\
+	                                             "PI_4__Density = %d\n"\
+	                                             "PI_5__Density = %d\n"\
+	                                             "PI_6__Density = %d\n"\
+	                                             "PI_7__Density = %d\n"\
+	                                             "PI_8__Density = %d\n"\
+	                                             "PI_9__Density = %d\n"\
+	                                             "PI_10_Density = %d\n", \
+            	               noise_res->IpiRpi,\
+            	               noise_res->ChannelNum,\
+            	               noise_res->PI_0_Density,\
+            	               noise_res->PI_1_Density,\
+            	               noise_res->PI_2_Density,\
+            	               noise_res->PI_3_Density,\
+            	               noise_res->PI_4_Density,\
+            	               noise_res->PI_5_Density,\
+            	               noise_res->PI_6_Density,\
+            	               noise_res->PI_7_Density,\
+            	               noise_res->PI_8_Density,\
+            	               noise_res->PI_9_Density,\
+            	               noise_res->PI_10_Density
+            	               );
+	            #endif
+	            break;
+	        case BeaconReport:
+	            break;
+	        case STAstatisticsReport:
+	            break;
+	        case LinkMeasurement:
+	            break;
+	    }
+	} else {
+	    wsm_printk(XRADIO_DBG_ERROR, "11K Measure(type=%d) Fail\n", measure_cmpl.MeasurementType);
+	}
+
+	return 0;
+
+underflow:
+	return -EINVAL;
+}
+/* TODO:COMBO:Make this perVIFF once mac80211 support is available */
+static int wsm_channel_switch_indication(struct xradio_common *hw_priv,
+						struct wsm_buf *buf)
+{
+	wsm_unlock_tx(hw_priv); /* Re-enable datapath */
+	WARN_ON(WSM_GET32(buf));
+
+	hw_priv->channel_switch_in_progress = 0;
+	wake_up(&hw_priv->channel_switch_done);
+
+
+	xradio_channel_switch_cb(hw_priv);
+	return 0;
+
+underflow:
+	return -EINVAL;
+}
+
+static int wsm_set_pm_indication(struct xradio_common *hw_priv,
+					struct wsm_buf *buf)
+{
+	wsm_oper_unlock(hw_priv);
+	return 0;
+}
+
+static int wsm_scan_complete_indication(struct xradio_common *hw_priv,
+					struct wsm_buf *buf)
+{
+	struct wsm_scan_complete arg;
+#ifdef ROAM_OFFLOAD
+	if(hw_priv->auto_scanning == 0)
+		wsm_oper_unlock(hw_priv);
+#else
+	wsm_oper_unlock(hw_priv);
+#endif /*ROAM_OFFLOAD*/
+
+	arg.status = WSM_GET32(buf);
+	arg.psm = WSM_GET8(buf);
+	arg.numChannels = WSM_GET8(buf);
+	xradio_scan_complete_cb(hw_priv, &arg);
+
+	return 0;
+
+underflow:
+	return -EINVAL;
+}
+
+static int wsm_find_complete_indication(struct xradio_common *hw_priv,
+					struct wsm_buf *buf)
+{
+	/* TODO: Implement me. */
+	//STUB();
+	return 0;
+}
+
+static int wsm_suspend_resume_indication(struct xradio_common *hw_priv,
+					 int interface_link_id,
+					 struct wsm_buf *buf)
+{
+		u32 flags;
+		struct wsm_suspend_resume arg;
+		struct xradio_vif *priv;
+
+		if (is_hardware_xradio(hw_priv)) {
+			int i;
+			arg.if_id = interface_link_id;
+			/* TODO:COMBO: Extract bitmap from suspend-resume
+			* TX indication */
+			xradio_for_each_vif(hw_priv, priv, i) {
+				if (!priv)
+					continue;
+				if (priv->join_status ==
+						XRADIO_JOIN_STATUS_AP) {
+					 arg.if_id = priv->if_id;
+					 break;
+				}
+				arg.link_id = 0;
+			}
+		} else {
+			arg.if_id = 0;
+			arg.link_id = interface_link_id;
+		}
+
+		flags = WSM_GET32(buf);
+		arg.stop = !(flags & 1);
+		arg.multicast = !!(flags & 8);
+		arg.queue = (flags >> 1) & 3;
+
+		priv = xrwl_hwpriv_to_vifpriv(hw_priv, arg.if_id);
+		if (unlikely(!priv)) {
+			wsm_printk(XRADIO_DBG_MSG, "suspend-resume indication"
+				   " for removed interface!\n");
+			return 0;
+		}
+		xradio_suspend_resume(priv, &arg);
+		spin_unlock(&priv->vif_lock);
+
+	return 0;
+
+underflow:
+	return -EINVAL;
+}
+
+
+/* ******************************************************************** */
+/* WSM TX								*/
+
+int wsm_cmd_send(struct xradio_common *hw_priv,
+		 struct wsm_buf *buf,
+		 void *arg, u16 cmd, long tmo, int if_id)
+{
+	size_t buf_len = buf->data - buf->begin;
+	int ret;
+
+	if (cmd == 0x0006 || cmd == 0x0005) /* Write/Read MIB */
+		wsm_printk(XRADIO_DBG_MSG, ">>> 0x%.4X [MIB: 0x%.4X] (%d)\n",
+			cmd, __le16_to_cpu(((__le16 *)buf->begin)[2]),
+			buf_len);
+	else
+		wsm_printk(XRADIO_DBG_MSG, ">>> 0x%.4X (%d)\n", cmd, buf_len);
+
+	if (unlikely(hw_priv->bh_error)) {
+		wsm_buf_reset(buf);
+		wsm_printk(XRADIO_DBG_ERROR, "bh error!>>> 0x%.4X (%d)\n", cmd, buf_len);
+		return -ETIMEDOUT;
+	}
+
+	/* Fill HI message header */
+	/* BH will add sequence number */
+
+	/* TODO:COMBO: Add if_id from  to the WSM header */
+	/* if_id == -1 indicates that command is HW specific,
+	 * eg. wsm_configuration which is called during driver initialzation
+	 *  (mac80211 .start callback called when first ifce is created. )*/
+
+	/* send hw specific commands on if 0 */
+	if (if_id == -1)
+		if_id = 0;
+
+	((__le16 *)buf->begin)[0] = __cpu_to_le16(buf_len);
+	((__le16 *)buf->begin)[1] = __cpu_to_le16(cmd |
+					((is_hardware_xradio(hw_priv)) ? (if_id << 6) : 0));
+
+	spin_lock(&hw_priv->wsm_cmd.lock);
+	BUG_ON(hw_priv->wsm_cmd.ptr);
+	hw_priv->wsm_cmd.done = 0;
+	hw_priv->wsm_cmd.ptr = buf->begin;
+	hw_priv->wsm_cmd.len = buf_len;
+	hw_priv->wsm_cmd.arg = arg;
+	hw_priv->wsm_cmd.cmd = cmd;
+	spin_unlock(&hw_priv->wsm_cmd.lock);
+
+	xradio_bh_wakeup(hw_priv);
+
+	if (unlikely(hw_priv->bh_error)) {
+		/* Do not wait for timeout if BH is dead. Exit immediately. */
+		ret = 0;
+	} else {
+		unsigned long wsm_cmd_max_tmo; 
+
+		/* Give start cmd a little more time */
+		if (unlikely(tmo == WSM_CMD_START_TIMEOUT))
+			wsm_cmd_max_tmo = WSM_CMD_START_TIMEOUT;
+		else
+			wsm_cmd_max_tmo = WSM_CMD_DEFAULT_TIMEOUT;
+
+		/*Set max timeout.*/
+		wsm_cmd_max_tmo = jiffies + wsm_cmd_max_tmo;
+
+		/* Firmware prioritizes data traffic over control confirm.
+		 * Loop below checks if data was RXed and increases timeout
+		 * accordingly. */
+		do {
+			/* It's safe to use unprotected access to wsm_cmd.done here */
+			ret = wait_event_timeout(hw_priv->wsm_cmd_wq, hw_priv->wsm_cmd.done, tmo);
+
+			/* check time since last rxed and max timeout.*/
+		} while (!ret && 
+		         time_before_eq(jiffies, hw_priv->rx_timestamp+tmo) && 
+		         time_before(jiffies, wsm_cmd_max_tmo));
+
+	}
+
+	if (unlikely(ret == 0)) {
+		u16 raceCheck;
+
+		spin_lock(&hw_priv->wsm_cmd.lock);
+		raceCheck = hw_priv->wsm_cmd.cmd;
+		hw_priv->wsm_cmd.arg = NULL;
+		hw_priv->wsm_cmd.ptr = NULL;
+		spin_unlock(&hw_priv->wsm_cmd.lock);
+
+		dev_err(hw_priv->pdev, "***CMD timeout!>>> 0x%.4X (%d), buf_use=%d, bh_state=%d\n",
+			   cmd, buf_len, hw_priv->hw_bufs_used, hw_priv->bh_error);
+		/* Race condition check to make sure _confirm is not called
+		 * after exit of _send */
+		if (raceCheck == 0xFFFF) {
+			/* If wsm_handle_rx got stuck in _confirm we will hang
+			 * system there. It's better than silently currupt
+			 * stack or heap, isn't it? */
+			BUG_ON(wait_event_timeout(
+					hw_priv->wsm_cmd_wq,
+					hw_priv->wsm_cmd.done,
+					WSM_CMD_LAST_CHANCE_TIMEOUT) <= 0);
+		}
+
+		/* Kill BH thread to report the error to the top layer. */
+		hw_priv->bh_error = 1;
+#ifdef BH_USE_SEMAPHORE
+		up(&hw_priv->bh_sem);
+#else
+		wake_up(&hw_priv->bh_wq);
+#endif
+		ret = -ETIMEDOUT;
+	} else {
+		spin_lock(&hw_priv->wsm_cmd.lock);
+		BUG_ON(!hw_priv->wsm_cmd.done);
+		ret = hw_priv->wsm_cmd.ret;
+		spin_unlock(&hw_priv->wsm_cmd.lock);
+	}
+	wsm_buf_reset(buf);
+	return ret;
+}
+
+/* ******************************************************************** */
+/* WSM TX port control							*/
+
+void wsm_lock_tx(struct xradio_common *hw_priv)
+{
+	down(&hw_priv->tx_lock_sem);
+	atomic_add(1, &hw_priv->tx_lock);
+	/* always check event if wsm_vif_lock_tx.*/
+	if (wsm_flush_tx(hw_priv)) 
+		wsm_printk(XRADIO_DBG_MSG, "TX is locked.\n");
+	up(&hw_priv->tx_lock_sem);
+}
+
+void wsm_vif_lock_tx(struct xradio_vif *priv)
+{
+	struct xradio_common *hw_priv = priv->hw_priv;
+	down(&hw_priv->tx_lock_sem);
+	if (atomic_add_return(1, &hw_priv->tx_lock) == 1) {
+		if (wsm_vif_flush_tx(priv))
+			wsm_printk(XRADIO_DBG_MSG, "TX is locked for"
+					" if_id %d.\n", priv->if_id);
+	}
+	up(&hw_priv->tx_lock_sem);
+}
+
+void wsm_lock_tx_async(struct xradio_common *hw_priv)
+{
+	if (atomic_add_return(1, &hw_priv->tx_lock) == 1)
+		wsm_printk(XRADIO_DBG_MSG, "TX is locked (async).\n");
+}
+
+bool wsm_flush_tx(struct xradio_common *hw_priv)
+{
+	long timeout = WSM_CMD_LAST_CHANCE_TIMEOUT;
+
+	/* Flush must be called with TX lock held. */
+	BUG_ON(!atomic_read(&hw_priv->tx_lock));
+
+	/* First check if we really need to do something.
+	 * It is safe to use unprotected access, as hw_bufs_used
+	 * can only decrements. */
+	if (!hw_priv->hw_bufs_used)
+		return true;
+
+	if (hw_priv->bh_error) {
+		/* In case of failure do not wait for magic. */
+		wsm_printk(XRADIO_DBG_ERROR, "Fatal error occured, "
+				"will not flush TX.\n");
+		return false;
+	} else {
+		/* Get "oldest" frame, if any frames stuck in firmware, 
+		   query all of them until max timeout. */
+		int num = hw_priv->hw_bufs_used + 1;
+		while (xradio_query_txpkt_timeout(hw_priv, XRWL_ALL_IFS, 
+		                                  0xffffffff, &timeout)) {
+			if (timeout < 0 || !num) {
+				/* Hmmm... Not good. Frame had stuck in firmware. */
+				wsm_printk(XRADIO_DBG_ERROR,
+						   "%s:hw_bufs_used=%d, num=%d, timeout=%ld\n",
+						   __func__, hw_priv->hw_bufs_used, num, timeout);
+				hw_priv->bh_error = 1;
+#ifdef BH_USE_SEMAPHORE
+				up(&hw_priv->bh_sem);
+#else
+				wake_up(&hw_priv->bh_wq);
+#endif
+				return false;
+			} else if (wait_event_timeout(hw_priv->bh_evt_wq, 
+			                       !hw_priv->hw_bufs_used, timeout) > 0) {
+				return true;
+			}
+			--num;
+		}
+		if (hw_priv->hw_bufs_used)
+			wsm_printk(XRADIO_DBG_ERROR, "%s:No pengding, but hw_bufs_used=%d\n",
+			           __func__, hw_priv->hw_bufs_used);
+		/* Ok, everything is flushed. */
+		return true;
+	}
+}
+
+bool wsm_vif_flush_tx(struct xradio_vif *priv)
+{
+	struct xradio_common *hw_priv = priv->hw_priv;
+	long timeout = WSM_CMD_LAST_CHANCE_TIMEOUT;
+	int if_id = priv->if_id;
+
+	/* Flush must be called with TX lock held. */
+	BUG_ON(!atomic_read(&hw_priv->tx_lock));
+
+	/* First check if we really need to do something.
+	 * It is safe to use unprotected access, as hw_bufs_used
+	 * can only decrements. */
+	if (!hw_priv->hw_bufs_used_vif[if_id])
+		return true;
+
+	if (hw_priv->bh_error) {
+		/* In case of failure do not wait for magic. */
+		wsm_printk(XRADIO_DBG_ERROR, "Fatal error occured, "
+				"will not flush TX.\n");
+		return false;
+	} else {
+		/* Get "oldest" frame, if any frames stuck in firmware, 
+		   query all of them until max timeout. */
+		int num = hw_priv->hw_bufs_used_vif[if_id] + 1;
+		while (xradio_query_txpkt_timeout(hw_priv, if_id, 0xffffffff, &timeout)) {
+			if (timeout < 0 || !num) {
+				/* Hmmm... Not good. Frame had stuck in firmware. */
+				wsm_printk(XRADIO_DBG_ERROR, "%s: if_id=%d, hw_bufs_used_vif=%d, num=%d\n", 
+				           __func__, if_id, hw_priv->hw_bufs_used_vif[priv->if_id],
+				           num);
+				hw_priv->bh_error = 1; 
+	#ifdef BH_USE_SEMAPHORE
+				up(&hw_priv->bh_sem);
+	#else
+				wake_up(&hw_priv->bh_wq);
+	#endif
+				return false;
+			} else if (wait_event_timeout(hw_priv->bh_evt_wq, 
+			              !hw_priv->hw_bufs_used_vif[if_id], timeout) > 0) {
+				return true;
+			}
+			--num;
+		}
+		if (hw_priv->hw_bufs_used_vif[if_id])
+			wsm_printk(XRADIO_DBG_ERROR, "%s:No pengding, but hw_bufs_used_vif=%d\n",
+			           __func__, hw_priv->hw_bufs_used_vif[priv->if_id]);
+		/* Ok, everything is flushed. */
+		return true;
+	}
+}
+
+
+void wsm_unlock_tx(struct xradio_common *hw_priv)
+{
+	int tx_lock;
+	if (hw_priv->bh_error)
+		wsm_printk(XRADIO_DBG_ERROR, "bh_error=%d, wsm_unlock_tx is unsafe\n",
+		           hw_priv->bh_error);
+	else {
+		tx_lock = atomic_sub_return(1, &hw_priv->tx_lock);
+		if (tx_lock < 0) {
+			BUG_ON(1);
+		} else if (tx_lock == 0) {
+			xradio_bh_wakeup(hw_priv);
+			wsm_printk(XRADIO_DBG_MSG, "TX is unlocked.\n");
+		}
+	}
+}
+
+/* ******************************************************************** */
+/* WSM RX								*/
+
+int wsm_handle_exception(struct xradio_common *hw_priv, u8 *data, size_t len)
+{
+	struct wsm_buf buf;
+	u32 reason;
+	u32 reg[18];
+	char fname[48];
+	int i = 0;
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+	struct xradio_vif *priv = NULL;
+#endif
+
+	static const char * const reason_str[] = {
+		"undefined instruction",
+		"prefetch abort",
+		"data abort",
+		"unknown error",
+	};
+
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+	/* Send the event upwards on the FW exception */
+	xradio_pm_stay_awake(&hw_priv->pm_state, 3*HZ);
+
+	spin_lock(&hw_priv->vif_list_lock);
+	xradio_for_each_vif(hw_priv, priv, i) {
+		if (!priv)
+			continue;
+		//ieee80211_driver_hang_notify(priv->vif, GFP_KERNEL);
+	}
+	spin_unlock(&hw_priv->vif_list_lock);
+#endif
+
+	buf.begin = buf.data = data;
+	buf.end = &buf.begin[len];
+
+	reason = WSM_GET32(&buf);
+	for (i = 0; i < ARRAY_SIZE(reg); ++i)
+		reg[i] = WSM_GET32(&buf);
+	WSM_GET(&buf, fname, sizeof(fname));
+
+	if (reason < 4) {
+		dev_err(hw_priv->pdev, "Firmware exception: %s.\n",
+		           reason_str[reason]);
+	} else {
+		dev_err(hw_priv->pdev, "Firmware assert at %.*s, line %d, reason=0x%x\n",
+			       sizeof(fname), fname, reg[1], reg[2]);
+	}
+
+	for (i = 0; i < 12; i += 4) {
+		dev_err(hw_priv->pdev, "Firmware:" \
+		           "R%d: 0x%.8X, R%d: 0x%.8X, R%d: 0x%.8X, R%d: 0x%.8X,\n",
+		           i + 0, reg[i + 0], i + 1, reg[i + 1],
+		           i + 2, reg[i + 2], i + 3, reg[i + 3]);
+	}
+	dev_err(hw_priv->pdev, "Firmware:" \
+	           "R12: 0x%.8X, SP: 0x%.8X, LR: 0x%.8X, PC: 0x%.8X,\n",
+	           reg[i + 0], reg[i + 1], reg[i + 2], reg[i + 3]);
+	i += 4;
+	dev_err(hw_priv->pdev, "Firmware:CPSR: 0x%.8X, SPSR: 0x%.8X\n",
+	           reg[i + 0], reg[i + 1]);
+	
+	return 0;
+
+underflow:
+	dev_err(hw_priv->pdev, "Firmware exception.\n");
+	print_hex_dump_bytes("Exception: ", DUMP_PREFIX_NONE, data, len);
+	return -EINVAL;
+}
+
+static int wsm_debug_indication(struct xradio_common *hw_priv,
+				                struct wsm_buf       *buf)
+{
+    //for only one debug item.
+    u32 dbg_id, buf_data=0; 
+    u16 dbg_buf_len; 
+    u8  dbg_len;
+    u8 *dbg_buf;
+    dbg_id = WSM_GET32(buf);
+
+    dbg_buf_len = buf->end - buf->data;
+    
+    if (dbg_id == 5) {
+	    do {
+	        dbg_buf_len = buf->end - buf->data;
+	        dbg_len = WSM_GET8(buf);
+	        if (dbg_len > dbg_buf_len - sizeof(dbg_len)) {
+	            wsm_printk(XRADIO_DBG_ERROR, "[FW]dbg_len     = %d\n", dbg_len);
+	            wsm_printk(XRADIO_DBG_ERROR, "[FW]dbg_buf_len = %d\n", dbg_buf_len);
+	            wsm_printk(XRADIO_DBG_ERROR, "[FW]debug ind err\n");
+	            //ret = -EINVAL;
+	            //return ret;
+	            
+	            
+	            break;
+	        }
+        
+        dbg_buf = buf->data;
+        //print it;
+        wsm_printk(XRADIO_DBG_ALWY,  "[FW-LOG] %s", dbg_buf);
+        //
+        buf->data += dbg_len;
+        
+    } while (buf->data < buf->end);
+	} else {
+		if (dbg_buf_len >= 4) {
+			buf_data = WSM_GET32(buf);
+			wsm_printk(XRADIO_DBG_ALWY,  "[FW-DEBUG] DbgId = %d, data = %d", dbg_id, buf_data);
+		} else {
+			wsm_printk(XRADIO_DBG_ALWY,  "[FW-DEBUG] DbgId = %d", dbg_id);
+		}
+	}
+
+    return 0;
+    
+underflow:
+	WARN_ON(1);
+	return -EINVAL;
+}
+
+int wsm_handle_rx(struct xradio_common *hw_priv, int id,
+		  struct wsm_hdr *wsm, struct sk_buff **skb_p)
+{
+	int ret = 0;
+	struct wsm_buf wsm_buf;
+	/* struct xradio_vif *priv = NULL; 	MRK: unused variable, see if 0 below */
+	/* int i = 0;				MRK: unused variable, see if 0 below */
+	int interface_link_id = (id >> 6) & 0x0F;
+#ifdef ROAM_OFFLOAD
+#if 0
+	struct xradio_vif *priv;
+	priv = xrwl_hwpriv_to_vifpriv(hw_priv, interface_link_id);
+	if (unlikely(!priv)) {
+		WARN_ON(1);
+		return 0;
+	}
+	spin_unlock(&priv->vif_lock);
+#endif
+#endif/*ROAM_OFFLOAD*/
+
+	/* Strip link id. */
+	id &= ~WSM_TX_LINK_ID(WSM_TX_LINK_ID_MAX);
+
+	wsm_buf.begin = (u8 *)&wsm[0];
+	wsm_buf.data = (u8 *)&wsm[1];
+	wsm_buf.end = &wsm_buf.begin[__le32_to_cpu(wsm->len)];
+
+	wsm_printk(XRADIO_DBG_MSG, "<<< 0x%.4X (%d)\n", id,
+			wsm_buf.end - wsm_buf.begin);
+
+#if defined(DGB_XRADIO_HWT)
+/***************************for HWT ********************************/
+	if (id == 0x0424) {
+		u16 TestID = *(u16 *)(wsm_buf.data);
+		if (TestID == 1)  //test frame confirm.
+			wsm_hwt_tx_confirm(hw_priv, &wsm_buf);
+		else {
+			spin_lock(&hw_priv->wsm_cmd.lock);
+			hw_priv->wsm_cmd.ret = *((u16 *)(wsm_buf.data) + 1);
+			hw_priv->wsm_cmd.done = 1;
+			spin_unlock(&hw_priv->wsm_cmd.lock);
+			wake_up(&hw_priv->wsm_cmd_wq);
+			wsm_printk(XRADIO_DBG_ALWY, "HWT TestID=0x%x Confirm ret=%d\n", 
+			           *(u16 *)(wsm_buf.data), hw_priv->wsm_cmd.ret);
+		}
+		return 0;
+	} else if (id == 0x0824) {
+		u16 TestID = *(u16 *)(wsm_buf.data);
+		switch (TestID) {
+		case 2:  //recieve a test frame.
+			wsm_hwt_rx_frames(hw_priv, &wsm_buf);
+			break;
+		case 3:  //enc test result.
+			wsm_hwt_enc_results(hw_priv, &wsm_buf);
+			break;
+		case 4:  //mic test result.
+			wsm_hwt_mic_results(hw_priv, &wsm_buf);
+			break;
+		default:
+			wsm_printk(XRADIO_DBG_ERROR, "HWT ERROR Indication TestID=0x%x\n", TestID);
+			break;
+		}
+		return 0;
+	}
+/***************************for HWT ********************************/
+#endif //DGB_XRADIO_HWT
+
+	if (id == 0x404) {
+		ret = wsm_tx_confirm(hw_priv, &wsm_buf, interface_link_id);
+#ifdef MCAST_FWDING
+#if 1 
+	} else if (id == 0x422) {
+		ret = wsm_give_buffer_confirm(hw_priv, &wsm_buf);
+#endif
+#endif
+
+	} else if (id == 0x41E) {
+		ret = wsm_multi_tx_confirm(hw_priv, &wsm_buf,
+					   interface_link_id);
+	} else if (id & 0x0400) {
+		void *wsm_arg;
+		u16 wsm_cmd;
+
+		/* Do not trust FW too much. Protection against repeated
+		 * response and race condition removal (see above). */
+		spin_lock(&hw_priv->wsm_cmd.lock);
+		wsm_arg = hw_priv->wsm_cmd.arg;
+		wsm_cmd = hw_priv->wsm_cmd.cmd &
+				~WSM_TX_LINK_ID(WSM_TX_LINK_ID_MAX);
+		hw_priv->wsm_cmd.cmd = 0xFFFF;
+		spin_unlock(&hw_priv->wsm_cmd.lock);
+
+		if (WARN_ON((id & ~0x0400) != wsm_cmd)) {
+			/* Note that any non-zero is a fatal retcode. */
+			ret = -EINVAL;
+			goto out;
+		}
+
+		switch (id) {
+		case 0x0409:
+			/* Note that wsm_arg can be NULL in case of timeout in
+			 * wsm_cmd_send(). */
+			if (likely(wsm_arg))
+				ret = wsm_configuration_confirm(hw_priv,
+								wsm_arg,
+								&wsm_buf);
+			break;
+		case 0x0405:
+			if (likely(wsm_arg))
+				ret = wsm_read_mib_confirm(hw_priv, wsm_arg,
+								&wsm_buf);
+			break;
+		case 0x0406:
+			if (likely(wsm_arg))
+				ret = wsm_write_mib_confirm(hw_priv, wsm_arg,
+							    &wsm_buf,
+							    interface_link_id);
+			break;
+		case 0x040B:
+			if (likely(wsm_arg))
+				ret = wsm_join_confirm(hw_priv, wsm_arg, &wsm_buf);
+			if (ret) 
+				wsm_printk(XRADIO_DBG_WARN, "Join confirm Failed!\n");
+			break;
+		case 0x040E: /* 11K measure*/
+			if (likely(wsm_arg))
+				ret = wsm_generic_confirm(hw_priv, wsm_arg, &wsm_buf);
+			if (ret) 
+				wsm_printk(XRADIO_DBG_ERROR, "[***HL***]11K Confirm Error\n");
+
+			break;
+
+#ifdef MCAST_FWDING
+		case 0x0423: /* req buffer cfm*/
+			if (likely(wsm_arg)){
+				xradio_for_each_vif(hw_priv, priv, i) {
+					if (priv && (priv->join_status == XRADIO_JOIN_STATUS_AP))
+						ret = wsm_request_buffer_confirm(priv,
+								wsm_arg, &wsm_buf);
+				}
+			}
+			break;
+#endif
+		case 0x0407: /* start-scan */
+#ifdef ROAM_OFFLOAD
+			if (hw_priv->auto_scanning) {
+				if (atomic_read(&hw_priv->scan.in_progress)) {
+					hw_priv->auto_scanning = 0;
+				}
+				else {
+					wsm_oper_unlock(hw_priv);
+					up(&hw_priv->scan.lock);
+				}
+			}
+#endif /*ROAM_OFFLOAD*/
+		case 0x0408: /* stop-scan */
+		case 0x040A: /* wsm_reset */
+		case 0x040C: /* add_key */
+		case 0x040D: /* remove_key */
+		case 0x0410: /* wsm_set_pm */
+		case 0x0411: /* set_bss_params */
+		case 0x0412: /* set_tx_queue_params */
+		case 0x0413: /* set_edca_params */
+		case 0x0416: /* switch_channel */
+		case 0x0417: /* start */
+		case 0x0418: /* beacon_transmit */
+		case 0x0419: /* start_find */
+		case 0x041A: /* stop_find */
+		case 0x041B: /* update_ie */
+		case 0x041C: /* map_link */
+			WARN_ON(wsm_arg != NULL);
+			ret = wsm_generic_confirm(hw_priv, wsm_arg, &wsm_buf);
+			if (ret)
+				wsm_printk(XRADIO_DBG_ERROR, 
+					"wsm_generic_confirm "
+    					"failed for request 0x%.4X.\n",
+					id & ~0x0400);
+			break;
+		default:
+			BUG_ON(1);
+		}
+
+		spin_lock(&hw_priv->wsm_cmd.lock);
+		hw_priv->wsm_cmd.ret = ret;
+		hw_priv->wsm_cmd.done = 1;
+		spin_unlock(&hw_priv->wsm_cmd.lock);
+		ret = 0; /* Error response from device should ne stop BH. */
+
+		wake_up(&hw_priv->wsm_cmd_wq);
+	} else if (id & 0x0800) {
+		switch (id) {
+		case 0x0801:
+			ret = wsm_startup_indication(hw_priv, &wsm_buf);
+			break;
+		case 0x0804:
+			ret = wsm_receive_indication(hw_priv, interface_link_id,
+					&wsm_buf, skb_p);
+			break;
+		case 0x0805:
+			ret = wsm_event_indication(hw_priv, &wsm_buf,
+					interface_link_id);
+			break;
+		case 0x0807:
+		    wsm_printk(XRADIO_DBG_ERROR, "[11K]wsm_measure_cmpl_indication\n");
+//		    wsm_printk(XRADIO_DBG_ERROR, "[11K]wsm->len = %d\n",wsm->len);
+			
+			ret = wsm_measure_cmpl_indication(hw_priv, &wsm_buf);
+			break;
+		case 0x080A:
+			ret = wsm_channel_switch_indication(hw_priv, &wsm_buf);
+			break;
+		case 0x0809:
+			ret = wsm_set_pm_indication(hw_priv, &wsm_buf);
+			break;
+		case 0x0806:
+#ifdef ROAM_OFFLOAD
+			if(hw_priv->auto_scanning && hw_priv->frame_rcvd) {
+				struct xradio_vif *priv;
+				hw_priv->frame_rcvd = 0;
+				priv = xrwl_hwpriv_to_vifpriv(hw_priv, hw_priv->scan.if_id);
+				if (unlikely(!priv)) {
+					WARN_ON(1);
+					return 0;
+				}
+					spin_unlock(&priv->vif_lock);
+				if (hw_priv->beacon) {
+					struct wsm_scan_complete *scan_cmpl = \
+						(struct wsm_scan_complete *) \
+						((u8 *)wsm + sizeof(struct wsm_hdr));
+					struct ieee80211_rx_status *rhdr = \
+						IEEE80211_SKB_RXCB(hw_priv->beacon);
+					rhdr->signal = (s8)scan_cmpl->reserved;
+					if (!priv->cqm_use_rssi) {
+						rhdr->signal = rhdr->signal / 2 - 110;
+					}
+					if (!hw_priv->beacon_bkp)
+						hw_priv->beacon_bkp = \
+						skb_copy(hw_priv->beacon, GFP_ATOMIC);
+					ieee80211_rx_irqsafe(hw_priv->hw, hw_priv->beacon);
+					hw_priv->beacon = hw_priv->beacon_bkp;
+
+					hw_priv->beacon_bkp = NULL;
+				}
+				wsm_printk(XRADIO_DBG_MSG, \
+				"Send Testmode Event.\n");
+				xradio_testmode_event(priv->hw->wiphy,
+					NL80211_CMD_NEW_SCAN_RESULTS, 0,
+					0, GFP_KERNEL);
+
+			}
+#endif /*ROAM_OFFLOAD*/
+			ret = wsm_scan_complete_indication(hw_priv, &wsm_buf);
+			break;
+		case 0x080B:
+			ret = wsm_find_complete_indication(hw_priv, &wsm_buf);
+			break;
+		case 0x080C:
+			ret = wsm_suspend_resume_indication(hw_priv,
+					interface_link_id, &wsm_buf);
+			break;
+		case 0x080E:
+			wsm_printk(XRADIO_DBG_MSG,  "wsm_debug_indication");
+			ret = wsm_debug_indication(hw_priv, &wsm_buf);
+			break;
+
+		default:
+			wsm_printk(XRADIO_DBG_ERROR,  "unknown Indmsg ID=0x%04x,len=%d\n", 
+			           wsm->id, wsm->len);
+			break;
+		}
+	} else {
+		WARN_ON(1);
+		ret = -EINVAL;
+	}
+out:
+	return ret;
+}
+
+static bool wsm_handle_tx_data(struct xradio_vif *priv,
+			       const struct wsm_tx *wsm,
+			       const struct ieee80211_tx_info *tx_info,
+			       struct xradio_txpriv *txpriv,
+			       struct xradio_queue *queue)
+{
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	bool handled = false;
+	const struct ieee80211_hdr *frame =
+		(struct ieee80211_hdr *) &((u8 *)wsm)[txpriv->offset];
+	__le16 fctl = frame->frame_control;
+	enum {
+		doProbe,
+		doDrop,
+		doJoin,
+		doOffchannel,
+		doWep,
+		doTx,
+	} action = doTx;
+
+	hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	frame =  (struct ieee80211_hdr *) &((u8 *)wsm)[txpriv->offset];
+	fctl  = frame->frame_control;
+
+	switch (priv->mode) {
+	case NL80211_IFTYPE_STATION:
+		if (unlikely(priv->bss_loss_status == XRADIO_BSS_LOSS_CHECKING && 
+			         priv->join_status     == XRADIO_JOIN_STATUS_STA) &&
+			         ieee80211_is_data(fctl)) {
+			spin_lock(&priv->bss_loss_lock);
+			priv->bss_loss_confirm_id = wsm->packetID;
+			priv->bss_loss_status = XRADIO_BSS_LOSS_CONFIRMING;
+			spin_unlock(&priv->bss_loss_lock);
+		} else if (unlikely((priv->join_status <= XRADIO_JOIN_STATUS_MONITOR) ||
+		           memcmp(frame->addr1, priv->join_bssid,sizeof(priv->join_bssid)))) {
+			if (ieee80211_is_auth(fctl))
+				action = doJoin;
+			else if ((ieee80211_is_deauth(fctl) || ieee80211_is_disassoc(fctl))&&
+			         priv->join_status < XRADIO_JOIN_STATUS_MONITOR)
+				action = doDrop;  //no need to send deauth when STA-unjoined, yangfh 2014-10-31 16:32:16.
+			else if (ieee80211_is_probe_req(fctl))
+				action = doTx;
+			else if (memcmp(frame->addr1, priv->join_bssid,
+					sizeof(priv->join_bssid)) &&
+					(priv->join_status ==
+					XRADIO_JOIN_STATUS_STA) &&
+					(ieee80211_is_data(fctl))) {
+				action = doDrop;
+			}
+			else if (priv->join_status >=
+					XRADIO_JOIN_STATUS_MONITOR)
+				action = doTx;
+			else if (get_interface_id_scanning(hw_priv) != -1) {
+				wsm_printk(XRADIO_DBG_WARN, "Scan ONGOING dropping"
+				           " offchannel eligible frame.\n");
+				action = doDrop;
+			} else {
+				if (ieee80211_is_probe_resp(fctl))
+					action = doDrop;
+				else
+					action = doOffchannel;
+				wsm_printk(XRADIO_DBG_WARN, "Offchannel fctl=0x%04x", fctl);
+			}
+		}
+		break;
+	case NL80211_IFTYPE_AP:
+		if (unlikely(!priv->join_status))
+			action = doDrop;
+		else if (unlikely(!(BIT(txpriv->raw_link_id) &
+				(BIT(0) | priv->link_id_map)))) {
+			wsm_printk(XRADIO_DBG_WARN, 
+					"A frame with expired link id "
+					"is dropped.\n");
+			action = doDrop;
+		}
+		if (xradio_queue_get_generation(wsm->packetID) >
+				XRADIO_MAX_REQUEUE_ATTEMPTS) {
+			/* HACK!!! WSM324 firmware has tendency to requeue
+			 * multicast frames in a loop, causing performance
+			 * drop and high power consumption of the driver.
+			 * In this situation it is better just to drop
+			 * the problematic frame. */
+			wsm_printk(XRADIO_DBG_WARN, 
+					"Too many attempts "
+					"to requeue a frame. "
+					"Frame is dropped, fctl=0x%04x.\n", fctl);
+			action = doDrop;
+		}
+		break;
+	case NL80211_IFTYPE_ADHOC:
+	case NL80211_IFTYPE_MESH_POINT:
+		//STUB();
+	case NL80211_IFTYPE_MONITOR:
+	default:
+		action = doDrop;
+		break;
+	}
+
+	if (action == doTx) {
+		if (unlikely(ieee80211_is_probe_req(fctl))) {
+			action = doProbe;
+		} else if ((fctl & __cpu_to_le32(IEEE80211_FCTL_PROTECTED)) &&
+			tx_info->control.hw_key &&
+			unlikely(tx_info->control.hw_key->keyidx !=
+					priv->wep_default_key_id) &&
+			(tx_info->control.hw_key->cipher ==
+					WLAN_CIPHER_SUITE_WEP40 ||
+			 tx_info->control.hw_key->cipher ==
+					WLAN_CIPHER_SUITE_WEP104)) {
+			action = doWep;
+		}
+	}
+
+	switch (action) {
+	case doProbe:
+	{
+		/* An interesting FW "feature". Device filters
+		 * probe responses.
+		 * The easiest way to get it back is to convert
+		 * probe request into WSM start_scan command. */
+		wsm_printk(XRADIO_DBG_MSG, \
+			"Convert probe request to scan.\n");
+		wsm_lock_tx_async(hw_priv);
+		hw_priv->pending_frame_id = __le32_to_cpu(wsm->packetID);
+		queue_delayed_work(hw_priv->workqueue,
+				&hw_priv->scan.probe_work, 0);
+		handled = true;
+	}
+	break;
+	case doDrop:
+	{
+		/* See detailed description of "join" below.
+		 * We are dropping everything except AUTH in non-joined mode. */
+		wsm_printk(XRADIO_DBG_MSG, "Drop frame (0x%.4X).\n", fctl);
+		BUG_ON(xradio_queue_remove(queue,
+			__le32_to_cpu(wsm->packetID)));
+		handled = true;
+	}
+	break;
+	case doJoin:
+	{
+		/* p2p should disconnect when sta try to join a different channel AP, 
+		 * because no good performance in this case.
+		 */
+		struct xradio_vif *p2p_tmp_vif = __xrwl_hwpriv_to_vifpriv(hw_priv, 1);
+		if (priv->if_id == 0 && p2p_tmp_vif) {
+			if (p2p_tmp_vif->join_status >= XRADIO_JOIN_STATUS_STA && 
+			    hw_priv->channel_changed) {
+				wsm_printk(XRADIO_DBG_WARN, "combo with different channels, p2p disconnect.\n");
+				wms_send_disassoc_to_self(hw_priv, p2p_tmp_vif);
+			}
+		}
+
+		/* There is one more interesting "feature"
+		 * in FW: it can't do RX/TX before "join".
+		 * "Join" here is not an association,
+		 * but just a syncronization between AP and STA.
+		 * priv->join_status is used only in bh thread and does
+		 * not require protection */
+		wsm_printk(XRADIO_DBG_NIY, "Issue join command.\n");
+		wsm_lock_tx_async(hw_priv);
+		hw_priv->pending_frame_id = __le32_to_cpu(wsm->packetID);
+		if (queue_work(hw_priv->workqueue, &priv->join_work) <= 0)
+			wsm_unlock_tx(hw_priv);
+		handled = true;
+	}
+	break;
+	case doOffchannel:
+	{
+		wsm_printk(XRADIO_DBG_MSG, "Offchannel TX request.\n");
+		wsm_lock_tx_async(hw_priv);
+		hw_priv->pending_frame_id = __le32_to_cpu(wsm->packetID);
+		if (queue_work(hw_priv->workqueue, &priv->offchannel_work) <= 0)
+			wsm_unlock_tx(hw_priv);
+		handled = true;
+	}
+	break;
+	case doWep:
+	{
+		wsm_printk(XRADIO_DBG_MSG, "Issue set_default_wep_key.\n");
+		wsm_lock_tx_async(hw_priv);
+		priv->wep_default_key_id = tx_info->control.hw_key->keyidx;
+		hw_priv->pending_frame_id = __le32_to_cpu(wsm->packetID);
+		if (queue_work(hw_priv->workqueue, &priv->wep_key_work) <= 0)
+			wsm_unlock_tx(hw_priv);
+		handled = true;
+	}
+	break;
+	case doTx:
+	{
+#if 0
+		/* Kept for history. If you want to implement wsm->more,
+		 * make sure you are able to send a frame after that. */
+		wsm->more = (count > 1) ? 1 : 0;
+		if (wsm->more) {
+			/* HACK!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+			 * It's undocumented in WSM spec, but XRADIO hangs
+			 * if 'more' is set and no TX is performed due to TX
+			 * buffers limitation. */
+			if (priv->hw_bufs_used + 1 ==
+					priv->wsm_caps.numInpChBufs)
+				wsm->more = 0;
+		}
+
+		/* BUG!!! FIXME: we can't use 'more' at all: we don't know
+		 * future. It could be a request from upper layer with TX lock
+		 * requirements (scan, for example). If "more" is set device
+		 * will not send data and wsm_tx_lock() will fail...
+		 * It's not obvious how to fix this deadlock. Any ideas?
+		 * As a workaround more is set to 0. */
+		wsm->more = 0;
+#endif /* 0 */
+
+		if (ieee80211_is_deauth(fctl) &&
+				priv->mode != NL80211_IFTYPE_AP) {
+			/* Shedule unjoin work */
+			wsm_printk(XRADIO_DBG_WARN, "Issue unjoin command(TX).\n");
+#if 0
+			wsm->more = 0;
+#endif /* 0 */
+			wsm_lock_tx_async(hw_priv);
+			if (queue_work(hw_priv->workqueue, &priv->unjoin_work) <= 0)
+				wsm_unlock_tx(hw_priv);
+		}
+	}
+	break;
+	}
+	return handled;
+}
+
+static int xradio_get_prio_queue(struct xradio_vif *priv,
+				 u32 link_id_map, int *total)
+{
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	static u32 urgent;
+	struct wsm_edca_queue_params *edca;
+	unsigned score, best = -1;
+	int winner = -1;
+	int queued;
+	int i;
+	urgent = BIT(priv->link_id_after_dtim) | BIT(priv->link_id_uapsd);
+
+	/* search for a winner using edca params */
+	for (i = 0; i < 4; ++i) {
+		queued = xradio_queue_get_num_queued(priv,
+				&hw_priv->tx_queue[i],
+				link_id_map);
+		if (!queued)
+			continue;
+		*total += queued;
+		edca = &priv->edca.params[i];
+		score = ((edca->aifns + edca->cwMin) << 16) +
+				(edca->cwMax - edca->cwMin) *
+				(prandom_u32() & 0xFFFF);
+		if (score < best && (winner < 0 || i != 3)) {
+			best = score;
+			winner = i;
+		}
+	}
+
+	/* override winner if bursting */
+	if (winner >= 0 && hw_priv->tx_burst_idx >= 0 &&
+			winner != hw_priv->tx_burst_idx &&
+			!xradio_queue_get_num_queued(priv,
+				&hw_priv->tx_queue[winner],
+				link_id_map & urgent) &&
+			xradio_queue_get_num_queued(priv,
+				&hw_priv->tx_queue[hw_priv->tx_burst_idx],
+				link_id_map))
+		winner = hw_priv->tx_burst_idx;
+
+	return winner;
+}
+
+static int wsm_get_tx_queue_and_mask(struct xradio_vif *priv,
+				     struct xradio_queue **queue_p,
+				     u32 *tx_allowed_mask_p,
+				     bool *more)
+{
+	struct xradio_common *hw_priv = xrwl_vifpriv_to_hwpriv(priv);
+	int idx;
+	u32 tx_allowed_mask;
+	int total = 0;
+
+	/* Search for a queue with multicast frames buffered */
+	if (priv->tx_multicast) {
+		tx_allowed_mask = BIT(priv->link_id_after_dtim);
+		idx = xradio_get_prio_queue(priv,
+				tx_allowed_mask, &total);
+		if (idx >= 0) {
+			*more = total > 1;
+			goto found;
+		}
+	}
+
+	/* Search for unicast traffic */
+	tx_allowed_mask = ~priv->sta_asleep_mask;
+	tx_allowed_mask |= BIT(priv->link_id_uapsd);
+	if (priv->sta_asleep_mask) {
+		tx_allowed_mask |= priv->pspoll_mask;
+		tx_allowed_mask &= ~BIT(priv->link_id_after_dtim);
+	} else {
+		tx_allowed_mask |= BIT(priv->link_id_after_dtim);
+	}
+	idx = xradio_get_prio_queue(priv,
+			tx_allowed_mask, &total);
+	if (idx < 0)
+		return -ENOENT;
+
+found:
+	*queue_p = &hw_priv->tx_queue[idx];
+	*tx_allowed_mask_p = tx_allowed_mask;
+	return 0;
+}
+
+int wsm_get_tx(struct xradio_common *hw_priv, u8 **data,
+	       size_t *tx_len, int *burst, int *vif_selected)
+{
+	struct wsm_tx *wsm = NULL;
+	struct ieee80211_tx_info *tx_info;
+	struct xradio_queue *queue = NULL;
+	int queue_num;
+	u32 tx_allowed_mask = 0;
+	struct xradio_txpriv *txpriv = NULL;
+	/*
+	 * Count was intended as an input for wsm->more flag.
+	 * During implementation it was found that wsm->more
+	 * is not usable, see details above. It is kept just
+	 * in case you would like to try to implement it again.
+	 */
+	int count = 0;
+	int if_pending = 1;
+
+	/* More is used only for broadcasts. */
+	bool more = false;
+
+	if (hw_priv->wsm_cmd.ptr) {
+		++count;
+		spin_lock(&hw_priv->wsm_cmd.lock);
+		BUG_ON(!hw_priv->wsm_cmd.ptr);
+		*data = hw_priv->wsm_cmd.ptr;
+		*tx_len = hw_priv->wsm_cmd.len;
+		*burst = 1;
+		*vif_selected = -1;
+		spin_unlock(&hw_priv->wsm_cmd.lock);
+	} else {
+		for (;;) {
+			int ret;
+			struct xradio_vif *priv;
+#if 0
+			int num_pending_vif0, num_pending_vif1;
+#endif
+			if (atomic_add_return(0, &hw_priv->tx_lock))
+				break;
+			/* Keep one buffer reserved for commands. Note
+			   that, hw_bufs_used has already been incremented
+			   before reaching here. */
+			if (hw_priv->hw_bufs_used >=
+					hw_priv->wsm_caps.numInpChBufs)
+				break;
+			priv = wsm_get_interface_for_tx(hw_priv);
+			/* go to next interface ID to select next packet */
+				hw_priv->if_id_selected ^= 1;
+
+			/* There might be no interface before add_interface
+			 * call */
+			if (!priv) {
+				if (if_pending) {
+					if_pending = 0;
+					continue;
+				}
+				break;
+			}
+
+#if 0
+			if (((priv->if_id == 0) &&
+			(hw_priv->hw_bufs_used_vif[0] >=
+						XRWL_FW_VIF0_THROTTLE)) ||
+			((priv->if_id == 1) &&
+			(hw_priv->hw_bufs_used_vif[1] >=
+						XRWL_FW_VIF1_THROTTLE))) {
+				spin_unlock(&priv->vif_lock);
+				if (if_pending) {
+					if_pending = 0;
+					continue;
+				}
+				break;
+			}
+#endif
+
+			/* This can be removed probably: xradio_vif will not
+			 * be in hw_priv->vif_list (as returned from
+			 * wsm_get_interface_for_tx) until it's fully
+			 * enabled, so statement above will take case of that*/
+			if (!atomic_read(&priv->enabled)) {
+				spin_unlock(&priv->vif_lock);
+				break;
+			}
+
+			/* TODO:COMBO: Find the next interface for which
+			* packet needs to be found */
+			spin_lock_bh(&priv->ps_state_lock);
+			ret = wsm_get_tx_queue_and_mask(priv, &queue,
+					&tx_allowed_mask, &more);
+			queue_num = queue - hw_priv->tx_queue;
+
+			if (priv->buffered_multicasts &&
+					(ret || !more) &&
+					(priv->tx_multicast ||
+					 !priv->sta_asleep_mask)) {
+				priv->buffered_multicasts = false;
+				if (priv->tx_multicast) {
+					priv->tx_multicast = false;
+					queue_work(hw_priv->workqueue,
+						&priv->multicast_stop_work);
+				}
+			}
+
+			spin_unlock_bh(&priv->ps_state_lock);
+
+			if (ret) {
+				spin_unlock(&priv->vif_lock);
+				if (if_pending == 1) {
+					if_pending = 0;
+					continue;
+				}
+				break;
+			}
+
+			if (xradio_queue_get(queue,
+					priv->if_id,
+					tx_allowed_mask,
+					&wsm, &tx_info, &txpriv)) {
+				spin_unlock(&priv->vif_lock);
+				if_pending = 0;
+				continue;
+			}
+
+// #ifdef ROC_DEBUG
+// 			{
+// 				struct ieee80211_hdr *hdr =
+// 				(struct ieee80211_hdr *)
+// 					&((u8 *)wsm)[txpriv->offset];
+// 
+// 				wsm_printk(XRADIO_DBG_ERROR, "QGET-1 %x, off_id %d,"
+// 					       " if_id %d\n",
+// 						hdr->frame_control,
+// 						txpriv->offchannel_if_id,
+// 						priv->if_id);
+// 			}
+// #else
+// 			{
+// 				struct ieee80211_hdr *hdr =
+// 				(struct ieee80211_hdr *)
+// 					&((u8 *)wsm)[txpriv->offset];
+// 
+// 				wsm_printk(XRADIO_DBG_ERROR, "QGET-1 %x, off_id %d,"
+// 						   " if_id %d\n",
+// 						hdr->frame_control,
+// 						txpriv->raw_if_id,
+// 						priv->if_id);
+// 			}
+// #endif
+
+			if (wsm_handle_tx_data(priv, wsm,
+					tx_info, txpriv, queue)) {
+				spin_unlock(&priv->vif_lock);
+				if_pending = 0;
+				continue;  /* Handled by WSM */
+			}
+
+			wsm->hdr.id &= __cpu_to_le16(
+					~WSM_TX_IF_ID(WSM_TX_IF_ID_MAX));
+			if (txpriv->offchannel_if_id)
+				wsm->hdr.id |= cpu_to_le16(
+					WSM_TX_IF_ID(txpriv->offchannel_if_id));
+			else
+				wsm->hdr.id |= cpu_to_le16(
+					WSM_TX_IF_ID(priv->if_id));
+
+			*vif_selected = priv->if_id;
+// #ifdef ROC_DEBUG
+// /* remand the roc debug. */
+// 			{
+// 				struct ieee80211_hdr *hdr =
+// 				(struct ieee80211_hdr *)
+// 					&((u8 *)wsm)[txpriv->offset];
+// 
+// 				wsm_printk(XRADIO_DBG_ERROR, "QGET-2 %x, off_id %d,"
+// 					       " if_id %d\n",
+// 						hdr->frame_control,
+// 						txpriv->offchannel_if_id,
+// 						priv->if_id);
+// 			}
+// #else
+// 			{
+// 				struct ieee80211_hdr *hdr =
+// 				(struct ieee80211_hdr *)
+// 					&((u8 *)wsm)[txpriv->offset];
+// 
+// 				wsm_printk(XRADIO_DBG_ERROR, "QGET-2 %x, off_id %d,"
+// 						   " if_id %d\n",
+// 						hdr->frame_control,
+// 						txpriv->raw_if_id,
+// 						priv->if_id);
+// 			}
+// #endif
+
+			priv->pspoll_mask &= ~BIT(txpriv->raw_link_id);
+
+			*data = (u8 *)wsm;
+			*tx_len = __le16_to_cpu(wsm->hdr.len);
+
+			/* allow bursting if txop is set */
+			if (priv->edca.params[queue_num].txOpLimit)
+				*burst = min(*burst,
+					(int)xradio_queue_get_num_queued(priv,
+						queue, tx_allowed_mask) + 1);
+			else
+				*burst = 1;
+
+			/* store index of bursting queue */
+			if (*burst > 1)
+				hw_priv->tx_burst_idx = queue_num;
+			else
+				hw_priv->tx_burst_idx = -1;
+
+			if (more) {
+				struct ieee80211_hdr *hdr =
+					(struct ieee80211_hdr *)
+					&((u8 *)wsm)[txpriv->offset];
+				if(strstr(&priv->ssid[0], "6.1.12")) {
+					if(hdr->addr1[0] & 0x01 ) {
+						hdr->frame_control |=
+						cpu_to_le16(IEEE80211_FCTL_MOREDATA);
+					}
+				}
+				else {
+					/* more buffered multicast/broadcast frames
+					*  ==> set MoreData flag in IEEE 802.11 header
+					*  to inform PS STAs */
+					hdr->frame_control |=
+					cpu_to_le16(IEEE80211_FCTL_MOREDATA);
+				}
+			}
+			wsm_printk(XRADIO_DBG_MSG, ">>> 0x%.4X (%d) %p %c\n",
+				0x0004, *tx_len, *data,
+				wsm->more ? 'M' : ' ');
+			++count;
+			spin_unlock(&priv->vif_lock);
+			break;
+		}
+	}
+
+	return count;
+}
+
+void wsm_txed(struct xradio_common *hw_priv, u8 *data)
+{
+	if (data == hw_priv->wsm_cmd.ptr) {
+		spin_lock(&hw_priv->wsm_cmd.lock);
+		hw_priv->wsm_cmd.ptr = NULL;
+		spin_unlock(&hw_priv->wsm_cmd.lock);
+	}
+}
+
+/* ******************************************************************** */
+/* WSM buffer								*/
+
+void wsm_buf_init(struct wsm_buf *buf)
+{
+	int size = (SDIO_BLOCK_SIZE<<1); //for sdd file big than SDIO_BLOCK_SIZE
+	BUG_ON(buf->begin);
+	buf->begin = kmalloc(size, GFP_KERNEL);
+	buf->end = buf->begin ? &buf->begin[size] : buf->begin;
+	wsm_buf_reset(buf);
+}
+
+void wsm_buf_deinit(struct wsm_buf *buf)
+{
+	if(buf->begin)
+		kfree(buf->begin);
+	buf->begin = buf->data = buf->end = NULL;
+}
+
+static void wsm_buf_reset(struct wsm_buf *buf)
+{
+	if (buf->begin) {
+		buf->data = &buf->begin[4];
+		*(u32 *)buf->begin = 0;
+	} else
+		buf->data = buf->begin;
+}
+
+static int wsm_buf_reserve(struct wsm_buf *buf, size_t extra_size)
+{
+	size_t pos = buf->data - buf->begin;
+	size_t size = pos + extra_size;
+
+
+	if (size & (SDIO_BLOCK_SIZE - 1)) {
+		size &= SDIO_BLOCK_SIZE;
+		size += SDIO_BLOCK_SIZE;
+	}
+
+	buf->begin = krealloc(buf->begin, size, GFP_KERNEL);
+	if (buf->begin) {
+		buf->data = &buf->begin[pos];
+		buf->end = &buf->begin[size];
+		return 0;
+	} else {
+		buf->end = buf->data = buf->begin;
+		return -ENOMEM;
+	}
+}
+
+static struct xradio_vif 
+	*wsm_get_interface_for_tx(struct xradio_common *hw_priv)
+{
+	struct xradio_vif *priv = NULL, *i_priv;
+	int i = hw_priv->if_id_selected;
+
+	if ( 1 /*TODO:COMBO*/) {
+		spin_lock(&hw_priv->vif_list_lock);
+		i_priv = hw_priv->vif_list[i] ? 
+		         xrwl_get_vif_from_ieee80211(hw_priv->vif_list[i]) : NULL;
+		if (i_priv) {
+			priv = i_priv;
+			spin_lock(&priv->vif_lock);
+		}
+		/* TODO:COMBO:
+		* Find next interface based on TX bitmap announced by the FW
+		* Find next interface based on load balancing */
+		spin_unlock(&hw_priv->vif_list_lock);
+	} else {
+		priv = xrwl_hwpriv_to_vifpriv(hw_priv, 0);
+	}
+
+	return priv;
+}
+
+static inline int get_interface_id_scanning(struct xradio_common *hw_priv)
+{
+	if (hw_priv->scan.req || hw_priv->scan.direct_probe)
+		return hw_priv->scan.if_id;
+	else
+		return -1;
+}
diff --git a/drivers/net/wireless/xradio/wsm.h b/drivers/net/wireless/xradio/wsm.h
new file mode 100644
index 0000000..8056abc
--- /dev/null
+++ b/drivers/net/wireless/xradio/wsm.h
@@ -0,0 +1,2354 @@
+/*
+ * wsm interfaces for XRadio drivers
+ *
+ * Copyright (c) 2013, XRadio
+ * Author: XRadio
+ *
+ * This program is free software; you can 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 XRADIO_WSM_H_INCLUDED
+#define XRADIO_WSM_H_INCLUDED
+
+#include <linux/spinlock.h>
+
+struct xradio_common;
+
+/* Bands */
+/* Radio band 2.412 -2.484 GHz. */
+#define WSM_PHY_BAND_2_4G		(0)
+
+/* Radio band 4.9375-5.8250 GHz. */
+#define WSM_PHY_BAND_5G			(1)
+
+/* Transmit rates */
+/* 1   Mbps            ERP-DSSS */
+#define WSM_TRANSMIT_RATE_1		(0)
+
+/* 2   Mbps            ERP-DSSS */
+#define WSM_TRANSMIT_RATE_2		(1)
+
+/* 5.5 Mbps            ERP-CCK, ERP-PBCC (Not supported) */
+/* #define WSM_TRANSMIT_RATE_5		(2) */
+
+/* 11  Mbps            ERP-CCK, ERP-PBCC (Not supported) */
+/* #define WSM_TRANSMIT_RATE_11		(3) */
+
+/* 22  Mbps            ERP-PBCC (Not supported) */
+/* #define WSM_TRANSMIT_RATE_22		(4) */
+
+/* 33  Mbps            ERP-PBCC (Not supported) */
+/* #define WSM_TRANSMIT_RATE_33		(5) */
+
+/* 6   Mbps   (3 Mbps) ERP-OFDM, BPSK coding rate 1/2 */
+#define WSM_TRANSMIT_RATE_6		(6)
+
+/* 9   Mbps (4.5 Mbps) ERP-OFDM, BPSK coding rate 3/4 */
+#define WSM_TRANSMIT_RATE_9		(7)
+
+/* 12  Mbps  (6 Mbps)  ERP-OFDM, QPSK coding rate 1/2 */
+#define WSM_TRANSMIT_RATE_12		(8)
+
+/* 18  Mbps  (9 Mbps)  ERP-OFDM, QPSK coding rate 3/4 */
+#define WSM_TRANSMIT_RATE_18		(9)
+
+/* 24  Mbps (12 Mbps)  ERP-OFDM, 16QAM coding rate 1/2 */
+#define WSM_TRANSMIT_RATE_24		(10)
+
+/* 36  Mbps (18 Mbps)  ERP-OFDM, 16QAM coding rate 3/4 */
+#define WSM_TRANSMIT_RATE_36		(11)
+
+/* 48  Mbps (24 Mbps)  ERP-OFDM, 64QAM coding rate 1/2 */
+#define WSM_TRANSMIT_RATE_48		(12)
+
+/* 54  Mbps (27 Mbps)  ERP-OFDM, 64QAM coding rate 3/4 */
+#define WSM_TRANSMIT_RATE_54		(13)
+
+/* 6.5 Mbps            HT-OFDM, BPSK coding rate 1/2 */
+#define WSM_TRANSMIT_RATE_HT_6		(14)
+
+/* 13  Mbps            HT-OFDM, QPSK coding rate 1/2 */
+#define WSM_TRANSMIT_RATE_HT_13		(15)
+
+/* 19.5 Mbps           HT-OFDM, QPSK coding rate 3/4 */
+#define WSM_TRANSMIT_RATE_HT_19		(16)
+
+/* 26  Mbps            HT-OFDM, 16QAM coding rate 1/2 */
+#define WSM_TRANSMIT_RATE_HT_26		(17)
+
+/* 39  Mbps            HT-OFDM, 16QAM coding rate 3/4 */
+#define WSM_TRANSMIT_RATE_HT_39		(18)
+
+/* 52  Mbps            HT-OFDM, 64QAM coding rate 2/3 */
+#define WSM_TRANSMIT_RATE_HT_52		(19)
+
+/* 58.5 Mbps           HT-OFDM, 64QAM coding rate 3/4 */
+#define WSM_TRANSMIT_RATE_HT_58		(20)
+
+/* 65  Mbps            HT-OFDM, 64QAM coding rate 5/6 */
+#define WSM_TRANSMIT_RATE_HT_65		(21)
+
+/* Scan types */
+/* Foreground scan */
+#define WSM_SCAN_TYPE_FOREGROUND	(0)
+
+/* Background scan */
+#define WSM_SCAN_TYPE_BACKGROUND	(1)
+
+/* Auto scan */
+#define WSM_SCAN_TYPE_AUTO		(2)
+
+/* Scan flags */
+/* Forced background scan means if the station cannot */
+/* enter the power-save mode, it shall force to perform a */
+/* background scan. Only valid when ScanType is */
+/* background scan. */
+#define WSM_SCAN_FLAG_FORCE_BACKGROUND	(BIT(0))
+
+/* The WLAN device scans one channel at a time so */
+/* that disturbance to the data traffic is minimized. */
+#define WSM_SCAN_FLAG_SPLIT_METHOD	(BIT(1))
+
+/* Preamble Type. Long if not set. */
+#define WSM_SCAN_FLAG_SHORT_PREAMBLE	(BIT(2))
+
+/* 11n Tx Mode. Mixed if not set. */
+#define WSM_SCAN_FLAG_11N_GREENFIELD	(BIT(3))
+
+#define WSM_FLAG_MAC_INSTANCE_1	(BIT(4))
+
+#define WSM_FLAG_MAC_INSTANCE_0	(~(BIT(4)))
+
+/* Scan constraints */
+/* Maximum number of channels to be scanned. */
+#define WSM_SCAN_MAX_NUM_OF_CHANNELS	(48)
+
+/* The maximum number of SSIDs that the device can scan for. */
+#define WSM_SCAN_MAX_NUM_OF_SSIDS	(2)
+
+/* Power management modes */
+/* 802.11 Active mode */
+#define WSM_PSM_ACTIVE			(0)
+
+/* 802.11 PS mode */
+#define WSM_PSM_PS			BIT(0)
+
+/* Fast Power Save bit */
+#define WSM_PSM_FAST_PS_FLAG		BIT(7)
+
+/* Dynamic aka Fast power save */
+#define WSM_PSM_FAST_PS			(BIT(0) | BIT(7))
+
+/* Undetermined */
+/* Note : Undetermined status is reported when the */
+/* NULL data frame used to advertise the PM mode to */
+/* the AP at Pre or Post Background Scan is not Acknowledged */
+#define WSM_PSM_UNKNOWN			BIT(1)
+
+/* Queue IDs */
+/* best effort/legacy */
+#define WSM_QUEUE_BEST_EFFORT		(0)
+
+/* background */
+#define WSM_QUEUE_BACKGROUND		(1)
+
+/* video */
+#define WSM_QUEUE_VIDEO			(2)
+
+/* voice */
+#define WSM_QUEUE_VOICE			(3)
+
+/* HT TX parameters */
+/* Non-HT */
+#define WSM_HT_TX_NON_HT		(0)
+
+/* Mixed format */
+#define WSM_HT_TX_MIXED			(1)
+
+/* Greenfield format */
+#define WSM_HT_TX_GREENFIELD		(2)
+
+/* STBC allowed */
+#define WSM_HT_TX_STBC			(BIT(7))
+
+/* EPTA prioirty flags for BT Coex */
+/* default epta priority */
+#define WSM_EPTA_PRIORITY_DEFAULT	4
+/* use for normal data */
+#define WSM_EPTA_PRIORITY_DATA		4
+/* use for connect/disconnect/roaming*/
+#define WSM_EPTA_PRIORITY_MGT		5
+/* use for action frames */
+#define WSM_EPTA_PRIORITY_ACTION	5
+/* use for AC_VI data */
+#define WSM_EPTA_PRIORITY_VIDEO		5
+/* use for AC_VO data */
+#define WSM_EPTA_PRIORITY_VOICE		6
+/* use for EAPOL exchange */
+#define WSM_EPTA_PRIORITY_EAPOL		7
+
+/* TX status */
+/* Frame was sent aggregated */
+/* Only valid for WSM_SUCCESS status. */
+#define WSM_TX_STATUS_AGGREGATION	(BIT(0))
+
+/* Host should requeue this frame later. */
+/* Valid only when status is WSM_REQUEUE. */
+#define WSM_TX_STATUS_REQUEUE		(BIT(1))
+
+/* Normal Ack */
+#define WSM_TX_STATUS_NORMAL_ACK	(0<<2)
+
+/* No Ack */
+#define WSM_TX_STATUS_NO_ACK		(1<<2)
+
+/* No explicit acknowledgement */
+#define WSM_TX_STATUS_NO_EXPLICIT_ACK	(2<<2)
+
+/* Block Ack */
+/* Only valid for WSM_SUCCESS status. */
+#define WSM_TX_STATUS_BLOCK_ACK		(3<<2)
+
+/* RX status */
+/* Unencrypted */
+#define WSM_RX_STATUS_UNENCRYPTED	(0<<0)
+
+/* WEP */
+#define WSM_RX_STATUS_WEP		(1<<0)
+
+/* TKIP */
+#define WSM_RX_STATUS_TKIP		(2<<0)
+
+/* AES */
+#define WSM_RX_STATUS_AES		(3<<0)
+
+/* WAPI */
+#define WSM_RX_STATUS_WAPI		(4<<0)
+
+/* Macro to fetch encryption subfield. */
+#define WSM_RX_STATUS_ENCRYPTION(status) ((status) & 0x07)
+
+/* Frame was part of an aggregation */
+#define WSM_RX_STATUS_AGGREGATE		(BIT(3))
+
+/* Frame was first in the aggregation */
+#define WSM_RX_STATUS_AGGREGATE_FIRST	(BIT(4))
+
+/* Frame was last in the aggregation */
+#define WSM_RX_STATUS_AGGREGATE_LAST	(BIT(5))
+
+/* Indicates a defragmented frame */
+#define WSM_RX_STATUS_DEFRAGMENTED	(BIT(6))
+
+/* Indicates a Beacon frame */
+#define WSM_RX_STATUS_BEACON		(BIT(7))
+
+/* Indicates STA bit beacon TIM field */
+#define WSM_RX_STATUS_TIM		(BIT(8))
+
+/* Indicates Beacon frame's virtual bitmap contains multicast bit */
+#define WSM_RX_STATUS_MULTICAST		(BIT(9))
+
+/* Indicates frame contains a matching SSID */
+#define WSM_RX_STATUS_MATCHING_SSID	(BIT(10))
+
+/* Indicates frame contains a matching BSSI */
+#define WSM_RX_STATUS_MATCHING_BSSI	(BIT(11))
+
+/* Indicates More bit set in Framectl field */
+#define WSM_RX_STATUS_MORE_DATA		(BIT(12))
+
+/* Indicates frame received during a measurement process */
+#define WSM_RX_STATUS_MEASUREMENT	(BIT(13))
+
+/* Indicates frame received as an HT packet */
+#define WSM_RX_STATUS_HT		(BIT(14))
+
+/* Indicates frame received with STBC */
+#define WSM_RX_STATUS_STBC		(BIT(15))
+
+/* Indicates Address 1 field matches dot11StationId */
+#define WSM_RX_STATUS_ADDRESS1		(BIT(16))
+
+/* Indicates Group address present in the Address 1 field */
+#define WSM_RX_STATUS_GROUP		(BIT(17))
+
+/* Indicates Broadcast address present in the Address 1 field */
+#define WSM_RX_STATUS_BROADCAST		(BIT(18))
+
+/* Indicates group key used with encrypted frames */
+#define WSM_RX_STATUS_GROUP_KEY		(BIT(19))
+
+/* Macro to fetch encryption key index. */
+#define WSM_RX_STATUS_KEY_IDX(status)	(((status >> 20)) & 0x0F)
+
+/* Frame Control field starts at Frame offset + 2 */
+#define WSM_TX_2BYTES_SHIFT		(BIT(7))
+
+/* Join mode */
+/* IBSS */
+#define WSM_JOIN_MODE_IBSS		(0)
+
+/* BSS */
+#define WSM_JOIN_MODE_BSS		(1)
+
+/* PLCP preamble type */
+/* For long preamble */
+#define WSM_JOIN_PREAMBLE_LONG		(0)
+
+/* For short preamble (Long for 1Mbps) */
+#define WSM_JOIN_PREAMBLE_SHORT		(1)
+
+/* For short preamble (Long for 1 and 2Mbps) */
+#define WSM_JOIN_PREAMBLE_SHORT_2	(2)
+
+/* Join flags */
+/* Unsynchronized */
+#define WSM_JOIN_FLAGS_UNSYNCRONIZED	BIT(0)
+/* The BSS owner is a P2P GO */
+#define WSM_JOIN_FLAGS_P2P_GO		BIT(1)
+/* Force to join BSS with the BSSID and the
+ * SSID specified without waiting for beacons. The
+ * ProbeForJoin parameter is ignored. */
+#define WSM_JOIN_FLAGS_FORCE		BIT(2)
+/* Give probe request/response higher
+ * priority over the BT traffic */
+#define WSM_JOIN_FLAGS_PRIO		BIT(3)
+
+/* Key types */
+#define WSM_KEY_TYPE_WEP_DEFAULT	(0)
+#define WSM_KEY_TYPE_WEP_PAIRWISE	(1)
+#define WSM_KEY_TYPE_TKIP_GROUP		(2)
+#define WSM_KEY_TYPE_TKIP_PAIRWISE	(3)
+#define WSM_KEY_TYPE_AES_GROUP		(4)
+#define WSM_KEY_TYPE_AES_PAIRWISE	(5)
+#define WSM_KEY_TYPE_WAPI_GROUP		(6)
+#define WSM_KEY_TYPE_WAPI_PAIRWISE	(7)
+
+/* Key indexes */
+#define WSM_KEY_MAX_INDEX		(10)
+
+/* ACK policy */
+#define WSM_ACK_POLICY_NORMAL		(0)
+#define WSM_ACK_POLICY_NO_ACK		(1)
+
+/* Start modes */
+#define WSM_START_MODE_AP		(0)	/* Mini AP */
+#define WSM_START_MODE_P2P_GO		(1)	/* P2P GO */
+#define WSM_START_MODE_P2P_DEV		(2)	/* P2P device */
+
+/* SetAssociationMode MIB flags */
+#define WSM_ASSOCIATION_MODE_USE_PREAMBLE_TYPE		(BIT(0))
+#define WSM_ASSOCIATION_MODE_USE_HT_MODE		(BIT(1))
+#define WSM_ASSOCIATION_MODE_USE_BASIC_RATE_SET		(BIT(2))
+#define WSM_ASSOCIATION_MODE_USE_MPDU_START_SPACING	(BIT(3))
+#define WSM_ASSOCIATION_MODE_SNOOP_ASSOC_FRAMES		(BIT(4))
+
+/* RcpiRssiThreshold MIB flags */
+#define WSM_RCPI_RSSI_THRESHOLD_ENABLE	(BIT(0))
+#define WSM_RCPI_RSSI_USE_RSSI		(BIT(1))
+#define WSM_RCPI_RSSI_DONT_USE_UPPER	(BIT(2))
+#define WSM_RCPI_RSSI_DONT_USE_LOWER	(BIT(3))
+
+/* Update-ie constants */
+#define WSM_UPDATE_IE_BEACON		(BIT(0))
+#define WSM_UPDATE_IE_PROBE_RESP	(BIT(1))
+#define WSM_UPDATE_IE_PROBE_REQ		(BIT(2))
+
+/* WSM events */
+/* Error */
+#define WSM_EVENT_ERROR			(0)
+
+/* BSS lost */
+#define WSM_EVENT_BSS_LOST		(1)
+
+/* BSS regained */
+#define WSM_EVENT_BSS_REGAINED		(2)
+
+/* Radar detected */
+#define WSM_EVENT_RADAR_DETECTED	(3)
+
+/* RCPI or RSSI threshold triggered */
+#define WSM_EVENT_RCPI_RSSI		(4)
+
+/* BT inactive */
+#define WSM_EVENT_BT_INACTIVE		(5)
+
+/* BT active */
+#define WSM_EVENT_BT_ACTIVE		(6)
+
+#define WSM_EVENT_PS_MODE_ERROR         (7)
+
+#define WSM_EVENT_INACTIVITY		(9)
+
+/* MAC Addr Filter */
+#define WSM_MIB_ID_MAC_ADDR_FILTER	0x1030
+
+/* MIB IDs */
+/* 4.1  dot11StationId */
+#define WSM_MIB_ID_DOT11_STATION_ID		0x0000
+
+/* 4.2  dot11MaxtransmitMsduLifeTime */
+#define WSM_MIB_ID_DOT11_MAX_TRANSMIT_LIFTIME	0x0001
+
+/* 4.3  dot11MaxReceiveLifeTime */
+#define WSM_MIB_ID_DOT11_MAX_RECEIVE_LIFETIME	0x0002
+
+/* 4.4  dot11SlotTime */
+#define WSM_MIB_ID_DOT11_SLOT_TIME		0x0003
+
+/* 4.5  dot11GroupAddressesTable */
+#define WSM_MIB_ID_DOT11_GROUP_ADDRESSES_TABLE	0x0004
+#define WSM_MAX_GRP_ADDRTABLE_ENTRIES		8
+
+/* 4.6  dot11WepDefaultKeyId */
+#define WSM_MIB_ID_DOT11_WEP_DEFAULT_KEY_ID	0x0005
+
+/* 4.7  dot11CurrentTxPowerLevel */
+#define WSM_MIB_ID_DOT11_CURRENT_TX_POWER_LEVEL	0x0006
+
+/* 4.8  dot11RTSThreshold */
+#define WSM_MIB_ID_DOT11_RTS_THRESHOLD		0x0007
+
+/* Huanglu add for firmware debug control */
+#define WSM_MIB_ID_FW_DEBUG_CONTROL		0x0008
+
+/* yangfh add for read/write registers from firmware*/
+#define WSM_MIB_ID_RW_FW_REG		0x0009
+
+/* yangfh add for Set max number of mpdus in a-mpdu*/
+#define WSM_MIB_ID_SET_AMPDU_NUM		0x000a
+
+/* Huanglu add for tx-ampdu-len-adaption */
+#define WSM_MIB_ID_SET_TALA_PARA		0x000b
+
+/* yangfh add for set TPA param */
+#define WSM_MIB_ID_SET_TPA_PARAM		0x000c
+
+/* 4.9  NonErpProtection */
+#define WSM_MIB_ID_NON_ERP_PROTECTION		0x1000
+
+/* 4.10 ArpIpAddressesTable */
+#define WSM_MIB_ID_ARP_IP_ADDRESSES_TABLE	0x1001
+#define WSM_MAX_ARP_IP_ADDRTABLE_ENTRIES	1
+
+/* 4.11 TemplateFrame */
+#define WSM_MIB_ID_TEMPLATE_FRAME		0x1002
+
+/* 4.12 RxFilter */
+#define WSM_MIB_ID_RX_FILTER			0x1003
+
+/* 4.13 BeaconFilterTable */
+#define WSM_MIB_ID_BEACON_FILTER_TABLE		0x1004
+
+/* 4.14 BeaconFilterEnable */
+#define WSM_MIB_ID_BEACON_FILTER_ENABLE		0x1005
+
+/* 4.15 OperationalPowerMode */
+#define WSM_MIB_ID_OPERATIONAL_POWER_MODE	0x1006
+
+/* 4.16 BeaconWakeUpPeriod */
+#define WSM_MIB_ID_BEACON_WAKEUP_PERIOD		0x1007
+
+/* 4.17 RcpiRssiThreshold */
+#define WSM_MIB_ID_RCPI_RSSI_THRESHOLD		0x1009
+
+/* 4.18 StatisticsTable */
+#define WSM_MIB_ID_STATISTICS_TABLE		0x100A
+
+/* 4.19 IbssPsConfig */
+#define WSM_MIB_ID_IBSS_PS_CONFIG		0x100B
+
+/* 4.20 CountersTable */
+#define WSM_MIB_ID_COUNTERS_TABLE		0x100C
+#define WSM_MIB_ID_AMPDUCOUNTERS_TABLE		0x1036
+#define WSM_MIB_ID_TXPIPE_TABLE		0x1037
+#define WSM_MIB_ID_BACKOFF_DBG		0x1038
+#define WSM_MIB_ID_BACKOFF_CTRL		0x1039
+
+//add yangfh for requery packet status
+#define WSM_MIB_ID_REQ_PKT_STATUS	0x1040
+
+//add yangfh for TPA debug informations
+#define WSM_MIB_ID_TPA_DEBUG_INFO	0x1041
+
+//add yangfh for tx power informations
+#define WSM_MIB_ID_TX_POWER_INFO	0x1042
+
+//add yangfh for some hardware information
+#define WSM_MIB_ID_HW_INFO	        0x1043
+
+/* 4.21 BlockAckPolicy */
+#define WSM_MIB_ID_BLOCK_ACK_POLICY		0x100E
+
+/* 4.22 OverrideInternalTxRate */
+#define WSM_MIB_ID_OVERRIDE_INTERNAL_TX_RATE	0x100F
+
+/* 4.23 SetAssociationMode */
+#define WSM_MIB_ID_SET_ASSOCIATION_MODE		0x1010
+
+/* 4.24 UpdateEptaConfigData */
+#define WSM_MIB_ID_UPDATE_EPTA_CONFIG_DATA	0x1011
+
+/* 4.25 SelectCcaMethod */
+#define WSM_MIB_ID_SELECT_CCA_METHOD		0x1012
+
+/* 4.26 SetUpasdInformation */
+#define WSM_MIB_ID_SET_UAPSD_INFORMATION	0x1013
+
+/* 4.27 SetAutoCalibrationMode  WBF00004073 */
+#define WSM_MIB_ID_SET_AUTO_CALIBRATION_MODE	0x1015
+
+/* 4.28 SetTxRateRetryPolicy */
+#define WSM_MIB_ID_SET_TX_RATE_RETRY_POLICY	0x1016
+
+/* 4.29 SetHostMessageTypeFilter */
+#define WSM_MIB_ID_SET_HOST_MSG_TYPE_FILTER	0x1017
+
+/* 4.30 P2PFindInfo */
+#define WSM_MIB_ID_P2P_FIND_INFO		0x1018
+
+/* 4.31 P2PPsModeInfo */
+#define WSM_MIB_ID_P2P_PS_MODE_INFO		0x1019
+
+/* 4.32 SetEtherTypeDataFrameFilter */
+#define WSM_MIB_ID_SET_ETHERTYPE_DATAFRAME_FILTER 0x101A
+
+/* 4.33 SetUDPPortDataFrameFilter */
+#define WSM_MIB_ID_SET_UDPPORT_DATAFRAME_FILTER	0x101B
+
+/* 4.34 SetMagicDataFrameFilter */
+#define WSM_MIB_ID_SET_MAGIC_DATAFRAME_FILTER	0x101C
+#define WSM_MIB_ID_SET_HOST_SLEEP	0x1050
+
+/* This is the end of specification. */
+
+/* 4.35 P2PDeviceInfo */
+#define WSM_MIB_ID_P2P_DEVICE_INFO		0x101D
+
+/* 4.36 SetWCDMABand */
+#define WSM_MIB_ID_SET_WCDMA_BAND		0x101E
+
+/* 4.37 GroupTxSequenceCounter */
+#define WSM_MIB_ID_GRP_SEQ_COUNTER		0x101F
+
+/* 4.38 ProtectedMgmtPolicy */
+#define WSM_MIB_ID_PROTECTED_MGMT_POLICY	0x1020
+
+/* 4.39 SetHtProtection */
+#define WSM_MID_ID_SET_HT_PROTECTION		0x1021
+
+/* 4.40 GPIO Command */
+#define WSM_MIB_ID_GPIO_COMMAND			0x1022
+
+/* 4.41 TSF Counter Value */
+#define WSM_MIB_ID_TSF_COUNTER			0x1023
+
+/* Test Purposes Only */
+#define WSM_MIB_ID_BLOCK_ACK_INFO		0x100D
+
+/* 4.42 UseMultiTxConfMessage */
+#define WSM_MIB_USE_MULTI_TX_CONF		0x1024
+
+/* 4.43 Keep-alive period */
+#define WSM_MIB_ID_KEEP_ALIVE_PERIOD		0x1025
+
+/* 4.44 Disable BSSID filter */
+#define WSM_MIB_ID_DISABLE_BSSID_FILTER		0x1026
+
+/* Inactivity */
+#define WSM_MIB_ID_SET_INACTIVITY		0x1035
+
+/* MAC Addr Filter */
+#define WSM_MIB_ID_MAC_ADDR_FILTER		0x1030
+
+#ifdef MCAST_FWDING
+/* 4.51 Set Forwarding Offload */
+#define WSM_MIB_ID_FORWARDING_OFFLOAD		0x1033
+#endif
+
+/* Frame template types */
+#define WSM_FRAME_TYPE_PROBE_REQUEST	(0)
+#define WSM_FRAME_TYPE_BEACON		(1)
+#define WSM_FRAME_TYPE_NULL		(2)
+#define WSM_FRAME_TYPE_QOS_NULL		(3)
+#define WSM_FRAME_TYPE_PS_POLL		(4)
+#define WSM_FRAME_TYPE_PROBE_RESPONSE	(5)
+#define WSM_FRAME_TYPE_ARP_REPLY        (6)
+
+#define WSM_FRAME_GREENFIELD		(0x80)	/* See 4.11 */
+
+/* Status */
+/* The WSM firmware has completed a request */
+/* successfully. */
+#define WSM_STATUS_SUCCESS              (0)
+
+/* This is a generic failure code if other error codes do */
+/* not apply. */
+#define WSM_STATUS_FAILURE              (1)
+
+/* A request contains one or more invalid parameters. */
+#define WSM_INVALID_PARAMETER           (2)
+
+/* The request cannot perform because the device is in */
+/* an inappropriate mode. */
+#define WSM_ACCESS_DENIED               (3)
+
+/* The frame received includes a decryption error. */
+#define WSM_STATUS_DECRYPTFAILURE       (4)
+
+/* A MIC failure is detected in the received packets. */
+#define WSM_STATUS_MICFAILURE           (5)
+
+/* The transmit request failed due to retry limit being */
+/* exceeded. */
+#define WSM_STATUS_RETRY_EXCEEDED       (6)
+
+/* The transmit request failed due to MSDU life time */
+/* being exceeded. */
+#define WSM_STATUS_TX_LIFETIME_EXCEEDED (7)
+
+/* The link to the AP is lost. */
+#define WSM_STATUS_LINK_LOST            (8)
+
+/* No key was found for the encrypted frame */
+#define WSM_STATUS_NO_KEY_FOUND         (9)
+
+/* Jammer was detected when transmitting this frame */
+#define WSM_STATUS_JAMMER_DETECTED      (10)
+
+/* The message should be requeued later. */
+/* This is applicable only to Transmit */
+#define WSM_REQUEUE                     (11)
+
+/* Advanced filtering options */
+#define WSM_MAX_FILTER_ELEMENTS		(4)
+
+#define WSM_FILTER_ACTION_IGNORE	(0)
+#define WSM_FILTER_ACTION_FILTER_IN	(1)
+#define WSM_FILTER_ACTION_FILTER_OUT	(2)
+
+#define WSM_FILTER_PORT_TYPE_DST	(0)
+#define WSM_FILTER_PORT_TYPE_SRC	(1)
+
+
+
+struct wsm_hdr {
+	__le16 len;
+	__le16 id;
+};
+
+#define WSM_TX_SEQ_MAX			(7)
+#define WSM_TX_SEQ(seq)			\
+		((seq & WSM_TX_SEQ_MAX) << 13)
+#define WSM_TX_LINK_ID_MAX		(0x0F)
+#define WSM_TX_LINK_ID(link_id)		\
+		((link_id & WSM_TX_LINK_ID_MAX) << 6)
+
+#define WSM_TX_IF_ID_MAX		(0x0F)
+#define WSM_TX_IF_ID(if_id)		\
+		((if_id & WSM_TX_IF_ID_MAX) << 6)
+
+#define MAX_BEACON_SKIP_TIME_MS 1000
+
+#ifdef FPGA_SETUP
+#define WSM_CMD_LAST_CHANCE_TIMEOUT (HZ * 9 / 2)
+#else
+#define WSM_CMD_LAST_CHANCE_TIMEOUT (HZ * 20 / 2)
+#endif
+#define WSM_CMD_EXTENDED_TIMEOUT (HZ * 20 / 2)
+
+#define WSM_RI_GET_PEER_ID_FROM_FLAGS(_f)         (((_f)&(0xF<<25)>>25))
+
+
+/* ******************************************************************** */
+/* WSM capcbility							*/
+#define WSM_FW_LABEL 128
+struct wsm_caps {
+	u16 numInpChBufs;
+	u16 sizeInpChBuf;
+	u16 hardwareId;
+	u16 hardwareSubId;
+	u16 firmwareCap;
+	u16 firmwareType;
+	u16 firmwareApiVer;
+	u16 firmwareBuildNumber;
+	u16 firmwareVersion;
+	char fw_label[WSM_FW_LABEL+2];
+	int firmwareReady;
+};
+
+/* ******************************************************************** */
+/* WSM commands								*/
+
+struct wsm_tx_power_range {
+	int min_power_level;
+	int max_power_level;
+	u32 stepping;
+};
+
+/* 3.1 */
+struct wsm_configuration {
+	/* [in] */ u32 dot11MaxTransmitMsduLifeTime;
+	/* [in] */ u32 dot11MaxReceiveLifeTime;
+	/* [in] */ u32 dot11RtsThreshold;
+	/* [in, out] */ u8 *dot11StationId;
+	/* [in] */ const void *dpdData;
+	/* [in] */ size_t dpdData_size;
+	/* [out] */ u8 dot11FrequencyBandsSupported;
+	/* [out] */ u32 supportedRateMask;
+	/* [out] */ struct wsm_tx_power_range txPowerRange[2];
+};
+
+int wsm_configuration(struct xradio_common *hw_priv,
+		      struct wsm_configuration *arg,
+		      int if_id);
+
+/* 3.3 */
+struct wsm_reset {
+	/* [in] */ int link_id;
+	/* [in] */ bool reset_statistics;
+};
+
+int wsm_reset(struct xradio_common *hw_priv, const struct wsm_reset *arg,
+	      int if_id);
+
+//add by yangfh
+void wsm_query_work(struct work_struct *work);
+
+/* 3.5 */
+int wsm_read_mib(struct xradio_common *hw_priv, u16 mibId, void *buf,
+		 size_t buf_size, size_t arg_size);
+
+/* 3.7 */
+int wsm_write_mib(struct xradio_common *hw_priv, u16 mibId, void *buf,
+		  size_t buf_size, int if_id);
+
+/* 3.9 */
+struct wsm_ssid {
+	u8 ssid[32];
+	u32 length;
+};
+
+struct wsm_scan_ch {
+	u16 number;
+	u32 minChannelTime;
+	u32 maxChannelTime;
+	u32 txPowerLevel;
+};
+
+/* 3.13 */
+struct wsm_scan_complete {
+	/* WSM_STATUS_... */
+	u32 status;
+
+	/* WSM_PSM_... */
+	u8 psm;
+
+	/* Number of channels that the scan operation completed. */
+	u8 numChannels;
+#ifdef ROAM_OFFLOAD
+	u16 reserved;
+#endif /*ROAM_OFFLOAD*/
+};
+
+/* 3.9 */
+struct wsm_scan {
+	/* WSM_PHY_BAND_... */
+	/* [in] */ u8 band;
+
+	/* WSM_SCAN_TYPE_... */
+	/* [in] */ u8 scanType;
+
+	/* WSM_SCAN_FLAG_... */
+	/* [in] */ u8 scanFlags;
+
+	/* WSM_TRANSMIT_RATE_... */
+	/* [in] */ u8 maxTransmitRate;
+
+	/* Interval period in TUs that the device shall the re- */
+	/* execute the requested scan. Max value supported by the device */
+	/* is 256s. */
+	/* [in] */ u32 autoScanInterval;
+
+	/* Number of probe requests (per SSID) sent to one (1) */
+	/* channel. Zero (0) means that none is send, which */
+	/* means that a passive scan is to be done. Value */
+	/* greater than zero (0) means that an active scan is to */
+	/* be done. */
+	/* [in] */ u32 numOfProbeRequests;
+
+	/* Number of channels to be scanned. */
+	/* Maximum value is WSM_SCAN_MAX_NUM_OF_CHANNELS. */
+	/* [in] */ u8 numOfChannels;
+
+	/* Number of SSID provided in the scan command (this */
+	/* is zero (0) in broadcast scan) */
+	/* The maximum number of SSIDs is WSM_SCAN_MAX_NUM_OF_SSIDS. */
+	/* [in] */ u8 numOfSSIDs;
+
+	/* The delay time (in microseconds) period */
+	/* before sending a probe-request. */
+	/* [in] */ u8 probeDelay;
+
+	/* SSIDs to be scanned [numOfSSIDs]; */
+	/* [in] */ struct wsm_ssid *ssids;
+
+	/* Channels to be scanned [numOfChannels]; */
+	/* [in] */ struct wsm_scan_ch *ch;
+};
+
+int wsm_scan(struct xradio_common *hw_priv, const struct wsm_scan *arg,
+			int if_id);
+
+/* 3.11 */
+int wsm_stop_scan(struct xradio_common *hw_priv, int if_id);
+
+/* 3.14 */
+struct wsm_tx_confirm {
+	/* Packet identifier used in wsm_tx. */
+	/* [out] */ u32 packetID;
+
+	/* WSM_STATUS_... */
+	/* [out] */ u32 status;
+
+	/* WSM_TRANSMIT_RATE_... */
+	/* [out] */ u8 txedRate;
+
+	/* The number of times the frame was transmitted */
+	/* without receiving an acknowledgement. */
+	/* [out] */ u8 ackFailures;
+
+	/* WSM_TX_STATUS_... */
+	/* [out] */ u16 flags;
+	
+	//rate feed back, add by yangfh
+	/* [out] */ u32 rate_try[3];
+
+	/* The total time in microseconds that the frame spent in */
+	/* the WLAN device before transmission as completed. */
+	/* [out] */ u32 mediaDelay;
+
+	/* The total time in microseconds that the frame spent in */
+	/* the WLAN device before transmission was started. */
+	/* [out] */ u32 txQueueDelay;
+
+	/* [out]*/ u32 link_id;
+
+	/*[out]*/ int if_id;
+};
+
+/* 3.15 */
+
+/* Note that ideology of wsm_tx struct is different against the rest of
+ * WSM API. wsm_hdr is /not/ a caller-adapted struct to be used as an input
+ * argument for WSM call, but a prepared bytestream to be sent to firmware.
+ * It is filled partly in xradio_tx, partly in low-level WSM code.
+ * Please pay attention once again: ideology is different.
+ *
+ * Legend:
+ * - [in]: xradio_tx must fill this field.
+ * - [wsm]: the field is filled by low-level WSM.
+ */
+struct wsm_tx {
+	/* common WSM header */
+	/* [in/wsm] */ struct wsm_hdr hdr;
+
+	/* Packet identifier that meant to be used in completion. */
+	/* [in] */ __le32 packetID;
+
+	/* WSM_TRANSMIT_RATE_... */
+	/* [in] */ u8 maxTxRate;
+
+	/* WSM_QUEUE_... */
+	/* [in] */ u8 queueId;
+
+	/* True: another packet is pending on the host for transmission. */
+	/* [wsm] */ u8 more;
+
+	/* Bit 0 = 0 - Start expiry time from first Tx attempt (default) */
+	/* Bit 0 = 1 - Start expiry time from receipt of Tx Request */
+	/* Bits 3:1  - PTA Priority */
+	/* Bits 6:4  - Tx Rate Retry Policy */
+	/* Bit 7 - Reserved */
+	/* [in] */ u8 flags;
+
+	/* Should be 0. */
+	/* [in] */ __le32 reserved;
+
+	/* The elapsed time in TUs, after the initial transmission */
+	/* of an MSDU, after which further attempts to transmit */
+	/* the MSDU shall be terminated. Overrides the global */
+	/* dot11MaxTransmitMsduLifeTime setting [optional] */
+	/* Device will set the default value if this is 0. */
+	/* [wsm] */ __le32 expireTime;
+
+	/* WSM_HT_TX_... */
+	/* [in] */ __le32 htTxParameters;
+};
+
+/* = sizeof(generic hi hdr) + sizeof(wsm hdr) + sizeof(alignment) */
+#define WSM_TX_EXTRA_HEADROOM (28)
+
+/* 3.16 */
+struct wsm_rx {
+	/* WSM_STATUS_... */
+	/* [out] */ u32 status;
+
+	/* Specifies the channel of the received packet. */
+	/* [out] */ u16 channelNumber;
+
+	/* WSM_TRANSMIT_RATE_... */
+	/* [out] */ u8 rxedRate;
+
+	/* This value is expressed in signed Q8.0 format for */
+	/* RSSI and unsigned Q7.1 format for RCPI. */
+	/* [out] */ u8 rcpiRssi;
+
+	/* WSM_RX_STATUS_... */
+	/* [out] */ u32 flags;
+
+	/* An 802.11 frame. */
+	/* [out] */ void *frame;
+
+	/* Size of the frame */
+	/* [out] */ size_t frame_size;
+
+	/* Link ID */
+	/* [out] */ int link_id;
+	/* [out] */ int if_id;
+};
+
+/* = sizeof(generic hi hdr) + sizeof(wsm hdr) */
+#define WSM_RX_EXTRA_HEADROOM (16)
+
+/* 3.17 */
+struct wsm_event {
+	/* WSM_STATUS_... */
+	/* [out] */ u32 eventId;
+
+	/* Indication parameters. */
+	/* For error indication, this shall be a 32-bit WSM status. */
+	/* For RCPI or RSSI indication, this should be an 8-bit */
+	/* RCPI or RSSI value. */
+	/* [out] */ u32 eventData;
+};
+
+struct xradio_wsm_event {
+	struct list_head link;
+	struct wsm_event evt;
+	u8 if_id;
+};
+
+/* 3.18 - 3.22 */
+/* Measurement. Skipped for now. Irrelevent. */
+
+
+/* 3.23 */
+struct wsm_join {
+	/* WSM_JOIN_MODE_... */
+	/* [in] */ u8 mode;
+
+	/* WSM_PHY_BAND_... */
+	/* [in] */ u8 band;
+
+	/* Specifies the channel number to join. The channel */
+	/* number will be mapped to an actual frequency */
+	/* according to the band */
+	/* [in] */ u16 channelNumber;
+
+	/* Specifies the BSSID of the BSS or IBSS to be joined */
+	/* or the IBSS to be started. */
+	/* [in] */ u8 bssid[6];
+
+	/* ATIM window of IBSS */
+	/* When ATIM window is zero the initiated IBSS does */
+	/* not support power saving. */
+	/* [in] */ u16 atimWindow;
+
+	/* WSM_JOIN_PREAMBLE_... */
+	/* [in] */ u8 preambleType;
+
+	/* Specifies if a probe request should be send with the */
+	/* specified SSID when joining to the network. */
+	/* [in] */ u8 probeForJoin;
+
+	/* DTIM Period (In multiples of beacon interval) */
+	/* [in] */ u8 dtimPeriod;
+
+	/* WSM_JOIN_FLAGS_... */
+	/* [in] */ u8 flags;
+
+	/* Length of the SSID */
+	/* [in] */ u32 ssidLength;
+
+	/* Specifies the SSID of the IBSS to join or start */
+	/* [in] */ u8 ssid[32];
+
+	/* Specifies the time between TBTTs in TUs */
+	/* [in] */ u32 beaconInterval;
+
+	/* A bit mask that defines the BSS basic rate set. */
+	/* [in] */ u32 basicRateSet;
+
+	/* Minimum transmission power level in units of 0.1dBm */
+	/* [out] */ int minPowerLevel;
+
+	/* Maximum transmission power level in units of 0.1dBm */
+	/* [out] */ int maxPowerLevel;
+};
+
+int wsm_join(struct xradio_common *hw_priv, struct wsm_join *arg, int if_id);
+
+/* 3.25 */
+struct wsm_set_pm {
+	/* WSM_PSM_... */
+	/* [in] */ u8 pmMode;
+
+	/* in unit of 500us; 0 to use default */
+	/* [in] */ u8 fastPsmIdlePeriod;
+
+	/* in unit of 500us; 0 to use default */
+	/* [in] */ u8 apPsmChangePeriod;
+
+	/* in unit of 500us; 0 to disable auto-pspoll */
+	/* [in] */ u8 minAutoPsPollPeriod;
+};
+
+int wsm_set_pm(struct xradio_common *hw_priv, const struct wsm_set_pm *arg,
+	       int if_id);
+
+/* 3.27 */
+struct wsm_set_pm_complete {
+	u8 psm;			/* WSM_PSM_... */
+};
+
+/* 3.28 */
+struct wsm_set_bss_params {
+	/* The number of lost consecutive beacons after which */
+	/* the WLAN device should indicate the BSS-Lost event */
+	/* to the WLAN host driver. */
+	u8 beaconLostCount;
+
+	/* The AID received during the association process. */
+	u16 aid;
+
+	/* The operational rate set mask */
+	u32 operationalRateSet;
+};
+
+int wsm_set_bss_params(struct xradio_common *hw_priv,
+		       const struct wsm_set_bss_params *arg, int if_id);
+
+/* 3.30 */
+struct wsm_add_key {
+	u8 type;		/* WSM_KEY_TYPE_... */
+	u8 entryIndex;		/* Key entry index: 0 -- WSM_KEY_MAX_INDEX */
+	u16 reserved;
+	union {
+		struct {
+			u8 peerAddress[6];	/* MAC address of the
+						 * peer station */
+			u8 reserved;
+			u8 keyLength;		/* Key length in bytes */
+			u8 keyData[16];		/* Key data */
+		} __packed wepPairwiseKey;
+		struct {
+			u8 keyId;		/* Unique per key identifier
+						 * (0..3) */
+			u8 keyLength;		/* Key length in bytes */
+			u16 reserved;
+			u8 keyData[16];		/* Key data */
+		} __packed wepGroupKey;
+		struct {
+			u8 peerAddress[6];	/* MAC address of the
+						 * peer station */
+			u8 reserved[2];
+			u8 tkipKeyData[16];	/* TKIP key data */
+			u8 rxMicKey[8];		/* Rx MIC key */
+			u8 txMicKey[8];		/* Tx MIC key */
+		} __packed tkipPairwiseKey;
+		struct {
+			u8 tkipKeyData[16];	/* TKIP key data */
+			u8 rxMicKey[8];		/* Rx MIC key */
+			u8 keyId;		/* Key ID */
+			u8 reserved[3];
+			u8 rxSeqCounter[8];	/* Receive Sequence Counter */
+		} __packed tkipGroupKey;
+		struct {
+			u8 peerAddress[6];	/* MAC address of the
+						 * peer station */
+			u16 reserved;
+			u8 aesKeyData[16];	/* AES key data */
+		} __packed aesPairwiseKey;
+		struct {
+			u8 aesKeyData[16];	/* AES key data */
+			u8 keyId;		/* Key ID */
+			u8 reserved[3];
+			u8 rxSeqCounter[8];	/* Receive Sequence Counter */
+		} __packed aesGroupKey;
+		struct {
+			u8 peerAddress[6];	/* MAC address of the
+						 * peer station */
+			u8 keyId;		/* Key ID */
+			u8 reserved;
+			u8 wapiKeyData[16];	/* WAPI key data */
+			u8 micKeyData[16];	/* MIC key data */
+		} __packed wapiPairwiseKey;
+		struct {
+			u8 wapiKeyData[16];	/* WAPI key data */
+			u8 micKeyData[16];	/* MIC key data */
+			u8 keyId;		/* Key ID */
+			u8 reserved[3];
+		} __packed wapiGroupKey;
+	} __packed;
+} __packed;
+
+int wsm_add_key(struct xradio_common *hw_priv, const struct wsm_add_key *arg,
+			int if_id);
+
+/* 3.32 */
+struct wsm_remove_key {
+	/* Key entry index : 0-10 */
+	u8 entryIndex;
+};
+
+int wsm_remove_key(struct xradio_common *hw_priv,
+		   const struct wsm_remove_key *arg, int if_id);
+
+/* 3.34 */
+struct wsm_set_tx_queue_params {
+	/* WSM_ACK_POLICY_... */
+	u8 ackPolicy;
+
+	/* Medium Time of TSPEC (in 32us units) allowed per */
+	/* One Second Averaging Period for this queue. */
+	u16 allowedMediumTime;
+
+	/* dot11MaxTransmitMsduLifetime to be used for the */
+	/* specified queue. */
+	u32 maxTransmitLifetime;
+};
+
+struct wsm_tx_queue_params {
+	/* NOTE: index is a linux queue id. */
+	struct wsm_set_tx_queue_params params[4];
+};
+
+#define WSM_TX_QUEUE_SET(queue_params, queue, ack_policy, allowed_time,     \
+			 max_life_time)					    \
+do {									    \
+	struct wsm_set_tx_queue_params *p = &(queue_params)->params[queue]; \
+	p->ackPolicy = (ack_policy);				\
+	p->allowedMediumTime = (allowed_time);				\
+	p->maxTransmitLifetime = (max_life_time);			\
+} while (0)
+
+int wsm_set_tx_queue_params(struct xradio_common *hw_priv,
+			    const struct wsm_set_tx_queue_params *arg,
+			    u8 id, int if_id);
+
+/* 3.36 */
+struct wsm_edca_queue_params {
+	/* CWmin (in slots) for the access class. */
+	/* [in] */ u16 cwMin;
+
+	/* CWmax (in slots) for the access class. */
+	/* [in] */ u16 cwMax;
+
+	/* AIFS (in slots) for the access class. */
+	/* [in] */ u8 aifns;
+
+	/* TX OP Limit (in microseconds) for the access class. */
+	/* [in] */ u16 txOpLimit;
+
+	/* dot11MaxReceiveLifetime to be used for the specified */
+	/* the access class. Overrides the global */
+	/* dot11MaxReceiveLifetime value */
+	/* [in] */ u32 maxReceiveLifetime;
+
+	/* UAPSD trigger support for the access class. */
+	/* [in] */ bool uapsdEnable;
+};
+
+struct wsm_edca_params {
+	/* NOTE: index is a linux queue id. */
+	struct wsm_edca_queue_params params[4];
+};
+
+#define TXOP_UNIT 32
+#define WSM_EDCA_SET(edca, queue, aifs, cw_min, cw_max, txop, life_time,\
+		uapsd)	\
+	do {							\
+		struct wsm_edca_queue_params *p = &(edca)->params[queue]; \
+		p->cwMin = (cw_min);				\
+		p->cwMax = (cw_max);				\
+		p->aifns = (aifs);				\
+		p->txOpLimit = ((txop) * TXOP_UNIT);		\
+		p->maxReceiveLifetime = (life_time);		\
+		p->uapsdEnable = (uapsd);			\
+	} while (0)
+
+int wsm_set_edca_params(struct xradio_common *hw_priv,
+			const struct wsm_edca_params *arg, int if_id);
+
+int wsm_set_uapsd_param(struct xradio_common *hw_priv,
+			const struct wsm_edca_params *arg);
+
+/* 3.38 */
+/* Set-System info. Skipped for now. Irrelevent. */
+
+/* 3.40 */
+struct wsm_switch_channel {
+	/* 1 - means the STA shall not transmit any further */
+	/* frames until the channel switch has completed */
+	/* [in] */ u8 channelMode;
+
+	/* Number of TBTTs until channel switch occurs. */
+	/* 0 - indicates switch shall occur at any time */
+	/* 1 - occurs immediately before the next TBTT */
+	/* [in] */ u8 channelSwitchCount;
+
+	/* The new channel number to switch to. */
+	/* Note this is defined as per section 2.7. */
+	/* [in] */ u16 newChannelNumber;
+};
+
+int wsm_switch_channel(struct xradio_common *hw_priv,
+		       const struct wsm_switch_channel *arg, int if_id);
+
+struct wsm_start {
+	/* WSM_START_MODE_... */
+	/* [in] */ u8 mode;
+
+	/* WSM_PHY_BAND_... */
+	/* [in] */ u8 band;
+
+	/* Channel number */
+	/* [in] */ u16 channelNumber;
+
+	/* Client Traffic window in units of TU */
+	/* Valid only when mode == ..._P2P */
+	/* [in] */ u32 CTWindow;
+
+	/* Interval between two consecutive */
+	/* beacon transmissions in TU. */
+	/* [in] */ u32 beaconInterval;
+
+	/* DTIM period in terms of beacon intervals */
+	/* [in] */ u8 DTIMPeriod;
+
+	/* WSM_JOIN_PREAMBLE_... */
+	/* [in] */ u8 preambleType;
+
+	/* The delay time (in microseconds) period */
+	/* before sending a probe-request. */
+	/* [in] */ u8 probeDelay;
+
+	/* Length of the SSID */
+	/* [in] */ u8 ssidLength;
+
+	/* SSID of the BSS or P2P_GO to be started now. */
+	/* [in] */ u8 ssid[32];
+
+	/* The basic supported rates for the MiniAP. */
+	/* [in] */ u32 basicRateSet;
+};
+
+int wsm_start(struct xradio_common *hw_priv, const struct wsm_start *arg,
+		int if_id);
+
+#if 0
+struct wsm_beacon_transmit {
+	/* 1: enable; 0: disable */
+	/* [in] */ u8 enableBeaconing;
+};
+
+int wsm_beacon_transmit(struct xradio_common *hw_priv,
+			const struct wsm_beacon_transmit *arg,
+			int if_id);
+#endif
+
+int wsm_start_find(struct xradio_common *hw_priv, int if_id);
+
+int wsm_stop_find(struct xradio_common *hw_priv, int if_id);
+
+struct wsm_suspend_resume {
+	/* See 3.52 */
+	/* Link ID */
+	/* [out] */ int link_id;
+	/* Stop sending further Tx requests down to device for this link */
+	/* [out] */ bool stop;
+	/* Transmit multicast Frames */
+	/* [out] */ bool multicast;
+	/* The AC on which Tx to be suspended /resumed. */
+	/* This is applicable only for U-APSD */
+	/* WSM_QUEUE_... */
+	/* [out] */ int queue;
+	/* [out] */ int if_id;
+};
+
+/* 3.54 Update-IE request. */
+struct wsm_update_ie {
+	/* WSM_UPDATE_IE_... */
+	/* [in] */ u16 what;
+	/* [in] */ u16 count;
+	/* [in] */ u8 *ies;
+	/* [in] */ size_t length;
+};
+
+int wsm_update_ie(struct xradio_common *hw_priv,
+		  const struct wsm_update_ie *arg, int if_id);
+
+/* 3.56 */
+struct wsm_map_link {
+	/* MAC address of the remote device */
+	/* [in] */ u8 mac_addr[6];
+	/* [in] */ u8 unmap;
+	/* [in] */ u8 link_id;
+};
+
+int wsm_map_link(struct xradio_common *hw_priv, const struct wsm_map_link *arg,
+		int if_id);
+
+#ifdef MCAST_FWDING
+
+/* 3.65	Give Buffer Request */
+int wsm_init_release_buffer_request(struct xradio_common *priv, u8 index);
+
+/* 3.65 fixed memory leakage by yangfh*/
+int wsm_deinit_release_buffer(struct xradio_common *hw_priv);
+
+/* 3.67	Request Buffer Request */
+int wsm_request_buffer_request(struct xradio_vif *priv,
+                                u8 *arg);
+#endif
+/* ******************************************************************** */
+/* MIB shortcats							*/
+#define XR_RRM 1
+#ifdef XR_RRM//RadioResourceMeasurement
+/* RadioResourceMeasurement Request*/
+#define MEAS_CCA         0
+#define MEAS_CHANNELLOAD 1
+typedef struct LMAC_MEAS_CHANNEL_LOAD_PARAMS_S
+{
+    u8    Reserved;
+    u8    ChannelLoadCCA;
+    u16   ChannelNum;
+    u16   RandomInterval;
+    u16   MeasurementDuration;
+    u32   MeasurementStartTimel;
+    u32   MeasurementStartTimeh;
+}LMAC_MEAS_CHANNEL_LOAD_PARAMS;
+
+#define MEAS_RPI 0
+#define MEAS_IPI 1
+
+typedef struct LMAC_MEAS_NOISE_HISTOGRAM_PARAMS_S
+{
+    u8    Reserved;
+    u8    IpiRpi;
+    u16   ChannelNum;
+    u16   RandomInterval;
+    u16   MeasurementDuration;
+    u32   MeasurementStartTimel;
+    u32   MeasurementStartTimeh;
+}LMAC_MEAS_NOISE_HISTOGRAM_PARAMS;
+
+#define LMAC_MAX_SSIDS       16 
+#define LMAC_MAX_SSID_LENGTH 32
+typedef struct LMAC_CHANNELS_S
+{
+    u32  ChannelNum;
+    u32  MinChannelTime;
+    u32  MaxChannelTime;
+    s32  TxPowerLevel;
+}LMAC_CHANNELS;
+
+typedef struct LMAC_SSIDS_S
+{
+    u32  SSIDLength;
+    u8   SSID[LMAC_MAX_SSID_LENGTH];
+}LMAC_SSIDS;
+
+typedef struct LMAC_MEAS_BEACON_PARAMS_S
+{
+    //u8    RegulatoryClass;
+    //u8    MeasurementMode;
+    //u16   ChannelNum;
+    u16   RandomInterval;
+    //u16   MeasurementDuration;
+    //u8    Bssid[6];
+    u16   Reserved;
+    //SCAN_PARAMETERS ScanParameters;
+    u8   Band;
+    u8   ScanType;
+    u8   ScanFlags;
+    u8   MaxTransmitRate;
+    u32  AutoScanInterval;
+    u8   NumOfProbeRequests;
+    u8   NumOfChannels;
+    u8   NumOfSSIDs;
+    u8   ProbeDelay;
+    LMAC_CHANNELS Channels;
+    LMAC_SSIDS    Ssids; // here for SCAN_PARAMETER sizing purposes
+}LMAC_MEAS_BEACON_PARAMS;
+
+typedef struct LMAC_MEAS_STA_STATS_PARAMS_S
+{
+    u8    PeerMacAddress[6];
+    u16   RandomInterval;
+    u16   MeasurementDuration;
+    u8    GroupId;
+    u8    Reserved;
+}LMAC_MEAS_STA_STATS_PARAMS;
+
+typedef struct LMAC_MEAS_LINK_MEASUREMENT_PARAMS_S
+{
+    u8    Reserved[4];
+}LMAC_MEAS_LINK_MEASUREMENT_PARAMS;
+
+typedef union LMAC_MEAS_REQUEST_U
+{
+    LMAC_MEAS_CHANNEL_LOAD_PARAMS     ChannelLoadParams;
+    LMAC_MEAS_NOISE_HISTOGRAM_PARAMS  NoisHistogramParams;
+    LMAC_MEAS_BEACON_PARAMS           BeaconParams;
+    LMAC_MEAS_STA_STATS_PARAMS        StaStatsParams;
+   LMAC_MEAS_LINK_MEASUREMENT_PARAMS LinkMeasurementParams;
+} LMAC_MEAS_REQUEST;
+
+// This struct is a copy of WSM_HI_START_MEASUREMENT_REQ, except that MsgLen and MsgId is not included
+typedef struct MEASUREMENT_PARAMETERS_S
+{
+//    u16           MsgLen;
+//    u16           MsgId;
+    s32           TxPowerLevel;
+    u8            DurationMandatory;
+    u8            MeasurementType;
+    u8            MeasurementRequestLength;
+    u8            Reserved[5];
+    LMAC_MEAS_REQUEST MeasurementRequest;
+}MEASUREMENT_PARAMETERS;
+
+/* RadioResourceMeasurement Result*/
+ typedef struct LMAC_MEAS_CHANNEL_LOAD_RESULTS_S
+{
+    u8   Reserved;
+    u8   ChannelLoadCCA;
+    u16  ChannelNum;
+    u32  ActualMeasurementStartTimel;
+    u32  ActualMeasurementStartTimeh;
+    u16  MeasurementDuration;
+    u8   CCAbusyFraction;
+    u8   ChannelLoad;
+}LMAC_MEAS_CHANNEL_LOAD_RESULTS;
+
+typedef struct LMAC_MEAS_NOISE_HISTOGRAM_RESULTS_S
+{
+    u16  Reserved;
+    u16  ChannelNum;
+    u32  ActualMeasurementStartTimel;
+    u32  ActualMeasurementStartTimeh;
+    u16  MeasurementDuration;
+    u8   AntennaID;
+    u8   IpiRpi;
+    u8   PI_0_Density;
+    u8   PI_1_Density;
+    u8   PI_2_Density;
+    u8   PI_3_Density;
+    u8   PI_4_Density;
+    u8   PI_5_Density;
+    u8   PI_6_Density;
+    u8   PI_7_Density;
+    u8   PI_8_Density;
+    u8   PI_9_Density;
+    u8   PI_10_Density;
+    u8   Reserved2;
+}LMAC_MEAS_NOISE_HISTOGRAM_RESULTS;
+
+typedef struct LMAC_MEAS_BEACON_RESULTS_S
+{
+    u16  MeasurementDuration;
+    u16  Reserved;
+    u32  StartTsfl;
+    u32  StartTsfh;
+    u32  Durationl;
+    u32  Durationh;
+    //SCAN_PARAMETERS ScanParameters;
+    u8   Band;
+    u8   ScanType;
+    u8   ScanFlags;
+    u8   MaxTransmitRate;
+    u32  AutoScanInterval;
+    u8   NumOfProbeRequests;
+    u8   NumOfChannels;
+    u8   NumOfSSIDs;
+    u8   ProbeDelay;
+    LMAC_CHANNELS Channels;
+    LMAC_SSIDS    Ssids;
+}LMAC_MEAS_BEACON_RESULTS;
+
+typedef struct LMAC_MEAS_STA_STATS_RESULTS_S
+{
+    u16  MeasurementDuration;
+    u8   GroupId;
+    u8   StatisticsGroupDataLength;
+    u8   StatisticsGroupData[52];
+}LMAC_MEAS_STA_STATS_RESULTS;
+
+typedef struct LMAC_MEAS_LINK_MEASUREMENT_RESULTS_S
+{
+    s16  TransmitPower;
+    u8   RxAntennaID;
+    u8   TxAntennaID;
+    s32  NoiseLeveldBm;
+    s8   LatestRssi;
+    u8   Reserved1;
+    u8   Reserved2;
+    u8   Reserved3;
+}LMAC_MEAS_LINK_MEASUREMENT_RESULTS;
+
+typedef union LMAC_MEAS_REPORT_U
+{
+    LMAC_MEAS_CHANNEL_LOAD_RESULTS     ChannelLoadResults;
+    LMAC_MEAS_NOISE_HISTOGRAM_RESULTS  NoiseHistogramResults;
+    LMAC_MEAS_BEACON_RESULTS           BeaconResults;
+    LMAC_MEAS_STA_STATS_RESULTS        StaStatsResults;
+    LMAC_MEAS_LINK_MEASUREMENT_RESULTS LinkMeasurementResults;
+}LMAC_MEAS_REPORT;
+
+// Note: eMeasurementTypes MUST match the #define WSM_MEASURE_TYPE_XXX from wsm_api.h
+typedef enum {
+    ChannelLoadMeasurement=0,
+    NoiseHistrogramMeasurement,
+    BeaconReport,
+    STAstatisticsReport,
+    LinkMeasurement
+}eMeasurementTypes;
+
+typedef struct MEASUREMENT_COMPLETE_S
+{
+//    u16          RandomInterval;      
+//    u16          Reserved0;          
+    u8           Dot11PowerMgmtMode;  // From here WSM_HI_MEASURE_CMPL_IND and MEASUREMENT_COMPLETE_S must be identical
+    u8           MeasurementType;
+    u16          MoreInd;   // Set to 1 if more indications are to follow for this measurement, otherwise 0;
+    u32          Status;
+    u8           MeasurementReportLength;
+    u8           Reserved2[3];
+    LMAC_MEAS_REPORT MeasurementReport;
+}MEASUREMENT_COMPLETE; // Note: must be 32 bit aligned
+
+#endif
+int wsm_11k_measure_requset(struct xradio_common  *hw_priv,
+                                               u8  measure_type,
+                                              u16  ChannelNum,
+                                              u16  Duration);
+
+
+static inline int wsm_set_fw_debug_control(struct xradio_common *hw_priv,
+				       int debug_control, int if_id)
+{
+	__le32 val = __cpu_to_le32(debug_control);
+	return wsm_write_mib(hw_priv, WSM_MIB_ID_FW_DEBUG_CONTROL,
+			     &val, sizeof(val), if_id);
+}
+
+static inline int wsm_set_host_sleep(struct xradio_common *hw_priv,
+				       u8 host_sleep, int if_id)
+{
+	return wsm_write_mib(hw_priv, WSM_MIB_ID_SET_HOST_SLEEP,
+			     &host_sleep, sizeof(host_sleep), if_id);
+}
+
+static inline int wsm_set_output_power(struct xradio_common *hw_priv,
+				       int power_level, int if_id)
+{
+	__le32 val = __cpu_to_le32(power_level);
+	return wsm_write_mib(hw_priv, WSM_MIB_ID_DOT11_CURRENT_TX_POWER_LEVEL,
+			     &val, sizeof(val), if_id);
+}
+
+static inline int wsm_set_beacon_wakeup_period(struct xradio_common *hw_priv,
+					       unsigned dtim_interval,
+					       unsigned listen_interval,
+					       int if_id)
+{
+	struct {
+		u8 numBeaconPeriods;
+		u8 reserved;
+		__le16 listenInterval;
+	} val = {
+	dtim_interval, 0, __cpu_to_le16(listen_interval)};
+	if (dtim_interval > 0xFF || listen_interval > 0xFFFF)
+		return -EINVAL;
+	else
+		return wsm_write_mib(hw_priv, WSM_MIB_ID_BEACON_WAKEUP_PERIOD,
+				     &val, sizeof(val), if_id);
+}
+
+struct wsm_rcpi_rssi_threshold {
+	u8 rssiRcpiMode;	/* WSM_RCPI_RSSI_... */
+	u8 lowerThreshold;
+	u8 upperThreshold;
+	u8 rollingAverageCount;
+};
+
+static inline int wsm_set_rcpi_rssi_threshold(struct xradio_common *hw_priv,
+					struct wsm_rcpi_rssi_threshold *arg,
+					int if_id)
+{
+	return wsm_write_mib(hw_priv, WSM_MIB_ID_RCPI_RSSI_THRESHOLD, arg,
+			     sizeof(*arg), if_id);
+}
+
+struct wsm_counters_table {
+	__le32 countPlcpErrors;
+	__le32 countFcsErrors;
+	__le32 countTxPackets;
+	__le32 countRxPackets;
+	__le32 countRxPacketErrors;
+	__le32 countRtsSuccess;
+	__le32 countRtsFailures;
+	__le32 countRxFramesSuccess;
+	__le32 countRxDecryptionFailures;
+	__le32 countRxMicFailures;
+	__le32 countRxNoKeyFailures;
+	__le32 countTxMulticastFrames;
+	__le32 countTxFramesSuccess;
+	__le32 countTxFrameFailures;
+	__le32 countTxFramesRetried;
+	__le32 countTxFramesMultiRetried;
+	__le32 countRxFrameDuplicates;
+	__le32 countAckFailures;
+	__le32 countRxMulticastFrames;
+	__le32 countRxCMACICVErrors;
+	__le32 countRxCMACReplays;
+	__le32 countRxMgmtCCMPReplays;
+	__le32 countRxBIPMICErrors;
+};
+
+
+struct wsm_ampducounters_table {
+	u32 countTxAMPDUs;
+	u32 countTxMPDUsInAMPDUs;
+	u32 countTxOctetsInAMPDUs_l32;
+	u32 countTxOctetsInAMPDUs_h32;
+	u32 countRxAMPDUs;
+	u32 countRxMPDUsInAMPDUs;
+	u32 countRxOctetsInAMPDUs_l32;
+	u32 countRxOctetsInAMPDUs_h32;
+	u32 countRxDelimeterCRCErrorCount;
+	u32 countImplictBARFailures;
+	u32 countExplictBARFailures;
+};
+
+struct wsm_txpipe_counter {
+    u32 count1;
+    u32 count2;
+    u32 count3;
+    u32 count4;
+    u32 count5;
+    u32 count6;
+    u32 count7;
+    u32 count8;
+    u32 count9;
+    u32 counta;
+};
+
+struct wsm_backoff_counter {
+    u32 count0;
+    u32 count1;
+    u32 count2;
+    u32 count3;
+    u32 count4;
+    u32 count5;
+    u32 count6;
+    u32 count7;
+    u32 count8;
+    u32 count9;
+};
+//add by yangfh for read/write fw registers
+#define WSM_REG_RW_F   BIT(0)  //0:read, 1:write
+#define WSM_REG_RET_F   BIT(1)  //results is valid.
+#define WSM_REG_BK_F   BIT(4)  //operate in block mode.
+
+struct reg_data {
+	u32 reg_addr;
+	u32 reg_val;
+};
+
+typedef struct tag_wsm_reg_w {
+	u16 flag;
+	u16 data_size;
+	struct reg_data arg[16];
+} WSM_REG_W;
+
+typedef struct tag_wsm_reg_r {
+	u16 flag;
+	u16 data_size;
+	u32 arg[16];
+} WSM_REG_R;
+
+struct wsm_backoff_ctrl {
+    u32 enable;
+    u32 min;
+    u32 max;
+};
+struct wsm_tala_para {
+    u32 para;
+    u32 thresh;
+};
+static inline int wsm_get_counters_table(struct xradio_common *hw_priv,
+					 struct wsm_counters_table *arg)
+{
+	return wsm_read_mib(hw_priv, WSM_MIB_ID_COUNTERS_TABLE,
+			arg, sizeof(*arg), 0);
+}
+
+static inline int wsm_get_ampducounters_table(struct xradio_common *hw_priv,
+					 struct wsm_ampducounters_table *arg)
+{
+	return wsm_read_mib(hw_priv, WSM_MIB_ID_AMPDUCOUNTERS_TABLE,
+			arg, sizeof(*arg), 0);
+}
+
+static inline int wsm_get_txpipe_table(struct xradio_common *hw_priv,
+					 struct wsm_txpipe_counter *arg)
+{
+	return wsm_read_mib(hw_priv, WSM_MIB_ID_TXPIPE_TABLE,
+			arg, sizeof(*arg), 0);
+}
+
+static inline int wsm_get_backoff_dbg(struct xradio_common *hw_priv,
+					 struct wsm_backoff_counter *arg)
+{
+	return wsm_read_mib(hw_priv, WSM_MIB_ID_BACKOFF_DBG,
+			arg, sizeof(*arg), 0);
+}
+
+static inline int wsm_set_backoff_ctrl(struct xradio_common *hw_priv,
+					 struct wsm_backoff_ctrl *arg)
+{
+	return wsm_write_mib(hw_priv, WSM_MIB_ID_BACKOFF_CTRL,
+			arg, sizeof(*arg), 0);
+}
+
+static inline int wsm_set_tala(struct xradio_common *hw_priv,
+					 struct wsm_tala_para *arg)
+{
+	return wsm_write_mib(hw_priv, WSM_MIB_ID_SET_TALA_PARA,
+			arg, sizeof(*arg), 0);
+}
+static inline int wsm_get_station_id(struct xradio_common *hw_priv, u8 *mac)
+{
+	return wsm_read_mib(hw_priv, WSM_MIB_ID_DOT11_STATION_ID, mac,
+			    ETH_ALEN, 0);
+}
+
+struct wsm_rx_filter {
+	bool promiscuous;
+	bool bssid;
+	bool fcs;
+	bool probeResponder;
+	bool keepalive;
+};
+
+static inline int wsm_set_rx_filter(struct xradio_common *hw_priv,
+				    const struct wsm_rx_filter *arg,
+				    int if_id)
+{
+	__le32 val = 0;
+	if (arg->promiscuous)
+		val |= __cpu_to_le32(BIT(0));
+	if (arg->bssid)
+		val |= __cpu_to_le32(BIT(1));
+	if (arg->fcs)
+		val |= __cpu_to_le32(BIT(2));
+	if (arg->probeResponder)
+		val |= __cpu_to_le32(BIT(3));
+	if (arg->keepalive)
+		val |= __cpu_to_le32(BIT(4));
+	return wsm_write_mib(hw_priv, WSM_MIB_ID_RX_FILTER, &val, sizeof(val),
+			if_id);
+}
+
+int wsm_set_probe_responder(struct xradio_vif *priv, bool enable);
+int wsm_set_keepalive_filter(struct xradio_vif *priv, bool enable);
+
+#define WSM_BEACON_FILTER_IE_HAS_CHANGED	BIT(0)
+#define WSM_BEACON_FILTER_IE_NO_LONGER_PRESENT	BIT(1)
+#define WSM_BEACON_FILTER_IE_HAS_APPEARED	BIT(2)
+
+struct wsm_beacon_filter_table_entry {
+	u8	ieId;
+	u8	actionFlags;
+	u8	oui[3];
+	u8	matchData[3];
+} __packed;
+
+struct wsm_beacon_filter_table {
+	__le32 numOfIEs;
+	struct wsm_beacon_filter_table_entry entry[10];
+} __packed;
+
+static inline int wsm_set_beacon_filter_table(struct xradio_common *hw_priv,
+					struct wsm_beacon_filter_table *ft,
+					int if_id)
+{
+	size_t size = __le32_to_cpu(ft->numOfIEs) *
+		     sizeof(struct wsm_beacon_filter_table_entry) +
+		     sizeof(__le32);
+
+	return wsm_write_mib(hw_priv, WSM_MIB_ID_BEACON_FILTER_TABLE, ft, size,
+			if_id);
+}
+
+#define WSM_BEACON_FILTER_ENABLE	BIT(0) /* Enable/disable beacon filtering */
+#define WSM_BEACON_FILTER_AUTO_ERP	BIT(1) /* If 1 FW will handle ERP IE changes internally */
+
+struct wsm_beacon_filter_control {
+	int enabled;
+	int bcn_count;
+};
+
+static inline int wsm_beacon_filter_control(struct xradio_common *hw_priv,
+					struct wsm_beacon_filter_control *arg,
+					int if_id)
+{
+	struct {
+		__le32 enabled;
+		__le32 bcn_count;
+	} val;
+	val.enabled = __cpu_to_le32(arg->enabled);
+	val.bcn_count = __cpu_to_le32(arg->bcn_count);
+	return wsm_write_mib(hw_priv, WSM_MIB_ID_BEACON_FILTER_ENABLE, &val,
+			     sizeof(val), if_id);
+}
+
+enum wsm_power_mode {
+	wsm_power_mode_active = 0,
+	wsm_power_mode_doze = 1,
+	wsm_power_mode_quiescent = 2,
+};
+
+struct wsm_operational_mode {
+	enum wsm_power_mode power_mode;
+	int disableMoreFlagUsage;
+	int performAntDiversity;
+};
+
+static const struct wsm_operational_mode defaultoperationalmode = {
+	.power_mode = wsm_power_mode_active,
+	.disableMoreFlagUsage = true,
+};
+
+static inline int wsm_set_operational_mode(struct xradio_common *hw_priv,
+					const struct wsm_operational_mode *arg,
+					int if_id)
+{
+	u32 val = arg->power_mode;
+	
+	if (arg->disableMoreFlagUsage)
+		val |= BIT(4);
+	if (arg->performAntDiversity)
+		val |= BIT(5);
+	return wsm_write_mib(hw_priv, WSM_MIB_ID_OPERATIONAL_POWER_MODE, &val,
+			     sizeof(val), if_id);
+}
+
+struct wsm_inactivity {
+	u8 max_inactivity;
+	u8 min_inactivity;
+};
+
+static inline int wsm_set_inactivity(struct xradio_common *hw_priv,
+					const struct wsm_inactivity *arg,
+					int if_id)
+{
+	struct {
+	       u8	min_inactive;
+	       u8	max_inactive;
+	       u16	reserved;
+	} val;
+
+	val.max_inactive = arg->max_inactivity;
+	val.min_inactive = arg->min_inactivity;
+	val.reserved = 0;
+
+	return wsm_write_mib(hw_priv, WSM_MIB_ID_SET_INACTIVITY, &val,
+			     sizeof(val), if_id);
+}
+
+struct wsm_template_frame {
+	u8 frame_type;
+	u8 rate;
+	bool disable;
+	struct sk_buff *skb;
+};
+
+static inline int wsm_set_template_frame(struct xradio_common *hw_priv,
+					 struct wsm_template_frame *arg,
+					 int if_id)
+{
+	int ret;
+	u8 *p = skb_push(arg->skb, 4);
+	p[0] = arg->frame_type;
+	p[1] = arg->rate;
+	if (arg->disable)
+		((u16 *) p)[1] = 0;
+	else
+		((u16 *) p)[1] = __cpu_to_le16(arg->skb->len - 4);
+	ret = wsm_write_mib(hw_priv, WSM_MIB_ID_TEMPLATE_FRAME, p,
+			    arg->skb->len, if_id);
+	skb_pull(arg->skb, 4);
+	return ret;
+}
+
+
+struct wsm_protected_mgmt_policy {
+	bool protectedMgmtEnable;
+	bool unprotectedMgmtFramesAllowed;
+	bool encryptionForAuthFrame;
+};
+
+static inline int
+wsm_set_protected_mgmt_policy(struct xradio_common *hw_priv,
+			      struct wsm_protected_mgmt_policy *arg,
+			      int if_id)
+{
+	__le32 val = 0;
+	int ret;
+	if (arg->protectedMgmtEnable)
+		val |= __cpu_to_le32(BIT(0));
+	if (arg->unprotectedMgmtFramesAllowed)
+		val |= __cpu_to_le32(BIT(1));
+	if (arg->encryptionForAuthFrame)
+		val |= __cpu_to_le32(BIT(2));
+	ret = wsm_write_mib(hw_priv, WSM_MIB_ID_PROTECTED_MGMT_POLICY, &val,
+			    sizeof(val), if_id);
+	return ret;
+}
+
+static inline int wsm_set_block_ack_policy(struct xradio_common *hw_priv,
+					   u8 blockAckTxTidPolicy,
+					   u8 blockAckRxTidPolicy,
+					   int if_id)
+{
+	struct {
+		u8 blockAckTxTidPolicy;
+		u8 reserved1;
+		u8 blockAckRxTidPolicy;
+		u8 reserved2;
+	} val = {
+		.blockAckTxTidPolicy = blockAckTxTidPolicy,
+		.blockAckRxTidPolicy = blockAckRxTidPolicy,
+	};
+	return wsm_write_mib(hw_priv, WSM_MIB_ID_BLOCK_ACK_POLICY, &val,
+			     sizeof(val), if_id);
+}
+
+struct wsm_association_mode {
+	u8 flags;		/* WSM_ASSOCIATION_MODE_... */
+	u8 preambleType;	/* WSM_JOIN_PREAMBLE_... */
+	u8 greenfieldMode;	/* 1 for greenfield */
+	u8 mpduStartSpacing;
+	__le32 basicRateSet;
+};
+
+static inline int wsm_set_association_mode(struct xradio_common *hw_priv,
+					   struct wsm_association_mode *arg,
+					   int if_id)
+{
+	return wsm_write_mib(hw_priv, WSM_MIB_ID_SET_ASSOCIATION_MODE, arg,
+			     sizeof(*arg), if_id);
+}
+
+struct wsm_set_tx_rate_retry_policy_header {
+	u8 numTxRatePolicies;
+	u8 reserved[3];
+} __packed;
+
+struct wsm_set_tx_rate_retry_policy_policy {
+	u8 policyIndex;
+	u8 shortRetryCount;
+	u8 longRetryCount;
+	u8 policyFlags;
+	u8 rateRecoveryCount;
+	u8 reserved[3];
+	__le32 rateCountIndices[3];
+} __packed;
+
+struct wsm_set_tx_rate_retry_policy {
+	struct wsm_set_tx_rate_retry_policy_header hdr;
+	struct wsm_set_tx_rate_retry_policy_policy tbl[8];
+} __packed;
+
+static inline int wsm_set_tx_rate_retry_policy(struct xradio_common *hw_priv,
+				struct wsm_set_tx_rate_retry_policy *arg,
+				int if_id)
+{
+	size_t size = sizeof(struct wsm_set_tx_rate_retry_policy_header) +
+	    arg->hdr.numTxRatePolicies *
+	    sizeof(struct wsm_set_tx_rate_retry_policy_policy);
+	return wsm_write_mib(hw_priv, WSM_MIB_ID_SET_TX_RATE_RETRY_POLICY, arg,
+			     size, if_id);
+}
+
+/* 4.32 SetEtherTypeDataFrameFilter */
+struct wsm_ether_type_filter_hdr {
+	u8 nrFilters;		/* Up to WSM_MAX_FILTER_ELEMENTS */
+	u8 reserved[3];
+} __packed;
+
+struct wsm_ether_type_filter {
+	u8 filterAction;	/* WSM_FILTER_ACTION_XXX */
+	u8 reserved;
+	__le16 etherType;	/* Type of ethernet frame */
+} __packed;
+
+static inline int wsm_set_ether_type_filter(struct xradio_common *hw_priv,
+				struct wsm_ether_type_filter_hdr *arg,
+				int if_id)
+{
+	size_t size = sizeof(struct wsm_ether_type_filter_hdr) +
+		arg->nrFilters * sizeof(struct wsm_ether_type_filter);
+	return wsm_write_mib(hw_priv, WSM_MIB_ID_SET_ETHERTYPE_DATAFRAME_FILTER,
+		arg, size, if_id);
+}
+
+
+/* 4.33 SetUDPPortDataFrameFilter */
+struct wsm_udp_port_filter_hdr {
+	u8 nrFilters;		/* Up to WSM_MAX_FILTER_ELEMENTS */
+	u8 reserved[3];
+} __packed;
+
+struct wsm_udp_port_filter {
+	u8 filterAction;	/* WSM_FILTER_ACTION_XXX */
+	u8 portType;		/* WSM_FILTER_PORT_TYPE_XXX */
+	__le16 udpPort;		/* Port number */
+} __packed;
+
+static inline int wsm_set_udp_port_filter(struct xradio_common *hw_priv,
+				struct wsm_udp_port_filter_hdr *arg,
+				int if_id)
+{
+	size_t size = sizeof(struct wsm_udp_port_filter_hdr) +
+		arg->nrFilters * sizeof(struct wsm_udp_port_filter);
+	return wsm_write_mib(hw_priv, WSM_MIB_ID_SET_UDPPORT_DATAFRAME_FILTER,
+		arg, size, if_id);
+}
+
+/* Undocumented MIBs: */
+/* 4.35 P2PDeviceInfo */
+#define D11_MAX_SSID_LEN		(32)
+
+struct wsm_p2p_device_type {
+	__le16 categoryId;
+	u8 oui[4];
+	__le16 subCategoryId;
+} __packed;
+
+struct wsm_p2p_device_info {
+	struct wsm_p2p_device_type primaryDevice;
+	u8 reserved1[3];
+	u8 devNameSize;
+	u8 localDevName[D11_MAX_SSID_LEN];
+	u8 reserved2[3];
+	u8 numSecDevSupported;
+	struct wsm_p2p_device_type secondaryDevices[0];
+} __packed;
+
+/* 4.36 SetWCDMABand - WO */
+struct wsm_cdma_band {
+	u8 WCDMA_Band;
+	u8 reserved[3];
+} __packed;
+
+/* 4.37 GroupTxSequenceCounter - RO */
+struct wsm_group_tx_seq {
+	__le32 bits_47_16;
+	__le16 bits_15_00;
+	__le16 reserved;
+} __packed;
+
+/* 4.39 SetHtProtection - WO */
+#define WSM_DUAL_CTS_PROT_ENB		(1 << 0)
+#define WSM_NON_GREENFIELD_STA		PRESENT(1 << 1)
+#define WSM_HT_PROT_MODE__NO_PROT	(0 << 2)
+#define WSM_HT_PROT_MODE__NON_MEMBER	(1 << 2)
+#define WSM_HT_PROT_MODE__20_MHZ	(2 << 2)
+#define WSM_HT_PROT_MODE__NON_HT_MIXED	(3 << 2)
+#define WSM_LSIG_TXOP_PROT_FULL		(1 << 4)
+#define WSM_LARGE_L_LENGTH_PROT		(1 << 5)
+
+struct wsm_ht_protection {
+	__le32 flags;
+} __packed;
+
+/* 4.40 GPIO Command - R/W */
+#define WSM_GPIO_COMMAND_SETUP	0
+#define WSM_GPIO_COMMAND_READ	1
+#define WSM_GPIO_COMMAND_WRITE	2
+#define WSM_GPIO_COMMAND_RESET	3
+#define WSM_GPIO_ALL_PINS	0xFF
+
+struct wsm_gpio_command {
+	u8 GPIO_Command;
+	u8 pin;
+	__le16 config;
+} __packed;
+
+/* 4.41 TSFCounter - RO */
+struct wsm_tsf_counter {
+	__le64 TSF_Counter;
+} __packed;
+
+/* 4.43 Keep alive period */
+struct wsm_keep_alive_period {
+	__le16 keepAlivePeriod;
+	u8 reserved[2];
+} __packed;
+
+static inline int wsm_keep_alive_period(struct xradio_common *hw_priv,
+					int period, int if_id)
+{
+	struct wsm_keep_alive_period arg = {
+		.keepAlivePeriod = __cpu_to_le16(period),
+	};
+	return wsm_write_mib(hw_priv, WSM_MIB_ID_KEEP_ALIVE_PERIOD,
+			&arg, sizeof(arg), if_id);
+};
+
+/* BSSID filtering */
+struct wsm_set_bssid_filtering {
+	u8 filter;
+	u8 reserved[3];
+} __packed;
+
+static inline int wsm_set_bssid_filtering(struct xradio_common *hw_priv,
+					  bool enabled, int if_id)
+{
+	struct wsm_set_bssid_filtering arg = {
+		.filter = !enabled,
+	};
+	return wsm_write_mib(hw_priv, WSM_MIB_ID_DISABLE_BSSID_FILTER,
+			&arg, sizeof(arg), if_id);
+}
+
+/* Multicat filtering - 4.5 */
+struct wsm_multicast_filter {
+	__le32 enable;
+	__le32 numOfAddresses;
+	u8 macAddress[WSM_MAX_GRP_ADDRTABLE_ENTRIES][ETH_ALEN];
+} __packed;
+
+/* Mac Addr Filter Info */
+struct wsm_mac_addr_info {
+	u8 filter_mode;
+	u8 address_mode;
+	u8 MacAddr[6];
+} __packed;
+
+/* Mac Addr Filter */
+struct wsm_mac_addr_filter {
+	u8 numfilter;
+	u8 action_mode;
+	u8 Reserved[2];
+	struct wsm_mac_addr_info macaddrfilter[0];
+} __packed;
+
+/* Broadcast Addr Filter */
+struct wsm_broadcast_addr_filter {
+	u8 action_mode;
+	u8 nummacaddr;
+	u8 filter_mode;
+	u8 address_mode;
+	u8 MacAddr[6];
+} __packed;
+
+static inline int wsm_set_multicast_filter(struct xradio_common *hw_priv,
+					   struct wsm_multicast_filter *fp,
+					   int if_id)
+{
+	return wsm_write_mib(hw_priv, WSM_MIB_ID_DOT11_GROUP_ADDRESSES_TABLE,
+			     fp, sizeof(*fp), if_id);
+}
+
+/* ARP IPv4 filtering - 4.10 */
+struct wsm_arp_ipv4_filter {
+	__le32 enable;
+	__be32 ipv4Address[WSM_MAX_ARP_IP_ADDRTABLE_ENTRIES];
+} __packed;
+
+
+static inline int wsm_set_arp_ipv4_filter(struct xradio_common *hw_priv,
+					  struct wsm_arp_ipv4_filter *fp,
+					  int if_id)
+{
+	return wsm_write_mib(hw_priv, WSM_MIB_ID_ARP_IP_ADDRESSES_TABLE,
+			    fp, sizeof(*fp), if_id);
+}
+
+/* P2P Power Save Mode Info - 4.31 */
+struct wsm_p2p_ps_modeinfo {
+	u8	oppPsCTWindow;
+	u8	count;
+	u8	reserved;
+	u8	dtimCount;
+	__le32	duration;
+	__le32	interval;
+	__le32	startTime;
+} __packed;
+
+static inline int wsm_set_p2p_ps_modeinfo(struct xradio_common *hw_priv,
+					  struct wsm_p2p_ps_modeinfo *mi,
+					  int if_id)
+{
+	return wsm_write_mib(hw_priv, WSM_MIB_ID_P2P_PS_MODE_INFO,
+			     mi, sizeof(*mi), if_id);
+}
+
+static inline int wsm_get_p2p_ps_modeinfo(struct xradio_common *hw_priv,
+					  struct wsm_p2p_ps_modeinfo *mi)
+{
+	return wsm_read_mib(hw_priv, WSM_MIB_ID_P2P_PS_MODE_INFO,
+			    mi, sizeof(*mi), 0);
+}
+
+/* UseMultiTxConfMessage */
+
+static inline int wsm_use_multi_tx_conf(struct xradio_common *hw_priv,
+					bool enabled, int if_id)
+{
+	__le32 arg = enabled ? __cpu_to_le32(1) : 0;
+
+	return wsm_write_mib(hw_priv, WSM_MIB_USE_MULTI_TX_CONF,
+			&arg, sizeof(arg), if_id);
+}
+
+
+/* 4.26 SetUpasdInformation */
+struct wsm_uapsd_info {
+	__le16 uapsdFlags;
+	__le16 minAutoTriggerInterval;
+	__le16 maxAutoTriggerInterval;
+	__le16 autoTriggerStep;
+};
+
+static inline int wsm_set_uapsd_info(struct xradio_common *hw_priv,
+				     struct wsm_uapsd_info *arg,
+				     int if_id)
+{
+	/* TODO:COMBO:UAPSD will be supported only on one interface */
+	return wsm_write_mib(hw_priv, WSM_MIB_ID_SET_UAPSD_INFORMATION,
+				arg, sizeof(*arg), if_id);
+}
+
+/* 4.22 OverrideInternalTxRate */
+struct wsm_override_internal_txrate {
+	u8 internalTxRate;
+	u8 nonErpInternalTxRate;
+	u8 reserved[2];
+} __packed;
+
+static inline int
+wsm_set_override_internal_txrate(struct xradio_common *hw_priv,
+				     struct wsm_override_internal_txrate *arg,
+				     int if_id)
+{
+	return wsm_write_mib(hw_priv, WSM_MIB_ID_OVERRIDE_INTERNAL_TX_RATE,
+				arg, sizeof(*arg), if_id);
+}
+#ifdef MCAST_FWDING
+/* 4.51 SetForwardingOffload */
+struct wsm_forwarding_offload {
+	u8 fwenable;
+	u8 flags;
+	u8 reserved[2];
+} __packed;
+
+static inline int wsm_set_forwarding_offlad(struct xradio_common *hw_priv,
+				     struct wsm_forwarding_offload *arg,int if_id)
+{
+	return wsm_write_mib(hw_priv, WSM_MIB_ID_FORWARDING_OFFLOAD,
+				arg, sizeof(*arg),if_id);
+}
+
+#endif
+/* ******************************************************************** */
+/* WSM TX port control							*/
+
+void wsm_lock_tx(struct xradio_common *hw_priv);
+void wsm_vif_lock_tx(struct xradio_vif *priv);
+void wsm_lock_tx_async(struct xradio_common *hw_priv);
+bool wsm_flush_tx(struct xradio_common *hw_priv);
+bool wsm_vif_flush_tx(struct xradio_vif *priv);
+void wsm_unlock_tx(struct xradio_common *hw_priv);
+
+/* ******************************************************************** */
+/* WSM / BH API								*/
+
+int wsm_handle_exception(struct xradio_common *hw_priv, u8 * data, size_t len);
+int wsm_handle_rx(struct xradio_common *hw_priv, int id, struct wsm_hdr *wsm,
+		  struct sk_buff **skb_p);
+void wms_send_deauth_to_self(struct xradio_common *hw_priv, struct xradio_vif *priv);
+void wms_send_disassoc_to_self(struct xradio_common *hw_priv, struct xradio_vif *priv);
+
+/* ******************************************************************** */
+/* wsm_buf API								*/
+
+struct wsm_buf {
+	u8 *begin;
+	u8 *data;
+	u8 *end;
+};
+
+void wsm_buf_init(struct wsm_buf *buf);
+void wsm_buf_deinit(struct wsm_buf *buf);
+
+/* ******************************************************************** */
+/* wsm_cmd API								*/
+
+struct wsm_cmd {
+	spinlock_t lock;
+	int done;
+	u8 *ptr;
+	size_t len;
+	void *arg;
+	int ret;
+	u16 cmd;
+};
+
+/* ******************************************************************** */
+/* WSM TX buffer access							*/
+
+int wsm_get_tx(struct xradio_common *hw_priv, u8 **data,
+	       size_t *tx_len, int *burst, int *vif_selected);
+void wsm_txed(struct xradio_common *hw_priv, u8 *data);
+
+/* ******************************************************************** */
+/* Queue mapping: WSM <---> linux					*/
+/* Linux: VO VI BE BK							*/
+/* WSM:   BE BK VI VO							*/
+
+static inline u8 wsm_queue_id_to_linux(u8 queueId)
+{
+	static const u8 queue_mapping[] = {
+		2, 3, 1, 0
+	};
+	return queue_mapping[queueId];
+}
+
+static inline u8 wsm_queue_id_to_wsm(u8 queueId)
+{
+	static const u8 queue_mapping[] = {
+		3, 2, 0, 1
+	};
+	return queue_mapping[queueId];
+}
+
+#endif /* XRADIO_HWIO_H_INCLUDED */
diff --git a/drivers/net/wireless/xradio/xradio.h b/drivers/net/wireless/xradio/xradio.h
new file mode 100644
index 0000000..d565db0
--- /dev/null
+++ b/drivers/net/wireless/xradio/xradio.h
@@ -0,0 +1,577 @@
+/*
+ * Common define of private data for XRadio drivers
+ *
+ * Copyright (c) 2013, XRadio
+ * Author: XRadio
+ *
+ * This program is free software; you can 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 XRADIO_H
+#define XRADIO_H
+
+#include <linux/wait.h>
+#include <linux/mutex.h>
+#include <linux/workqueue.h>
+#include <linux/sched.h>
+#include <linux/atomic.h>
+#include <net/mac80211.h>
+
+//Macroses for Driver parameters.
+#define XRWL_MAX_QUEUE_SZ    (128)
+#define AC_QUEUE_NUM           4
+
+#define XRWL_MAX_VIFS        (2)
+#define XRWL_GENERIC_IF_ID   (2)
+#define XRWL_HOST_VIF0_11N_THROTTLE   (58)  //(XRWL_MAX_QUEUE_SZ/(XRWL_MAX_VIFS-1))*0.9
+#define XRWL_HOST_VIF1_11N_THROTTLE   (58)  //(XRWL_MAX_QUEUE_SZ/(XRWL_MAX_VIFS-1))*0.9
+#define XRWL_HOST_VIF0_11BG_THROTTLE  (35)  //XRWL_HOST_VIF0_11N_THROTTLE*0.6 = 35
+#define XRWL_HOST_VIF1_11BG_THROTTLE  (35)  //XRWL_HOST_VIF0_11N_THROTTLE*0.6 = 35
+#if 0
+#define XRWL_FW_VIF0_THROTTLE         (15)
+#define XRWL_FW_VIF1_THROTTLE         (15)
+#endif
+
+#define IEEE80211_FCTL_WEP      0x4000
+#define IEEE80211_QOS_DATAGRP   0x0080
+#define WSM_KEY_MAX_IDX         20
+
+#include "queue.h"
+#include "wsm.h"
+#include "scan.h"
+#include "tx.h"
+#include "ht.h"
+#include "pm.h"
+#include "fwio.h"
+
+/* #define ROC_DEBUG */
+/* hidden ssid is only supported when separate probe resp IE
+   configuration is supported */
+#ifdef PROBE_RESP_EXTRA_IE
+#define HIDDEN_SSID   1
+#endif
+
+#define XRADIO_MAX_CTRL_FRAME_LEN  (0x1000)
+
+#define MAX_STA_IN_AP_MODE         (14)
+#define WLAN_LINK_ID_MAX           (MAX_STA_IN_AP_MODE + 3)
+
+#define XRADIO_MAX_STA_IN_AP_MODE   (5)
+#define XRADIO_MAX_REQUEUE_ATTEMPTS (5)
+#define XRADIO_LINK_ID_UNMAPPED     (15)
+#define XRADIO_MAX_TID              (8)
+
+#define XRADIO_TX_BLOCK_ACK_ENABLED_FOR_ALL_TID    (0x3F)
+#define XRADIO_RX_BLOCK_ACK_ENABLED_FOR_ALL_TID    (0x3F)
+#define XRADIO_RX_BLOCK_ACK_ENABLED_FOR_BE_TID \
+       (XRADIO_TX_BLOCK_ACK_ENABLED_FOR_ALL_TID & 0x01)
+#define XRADIO_TX_BLOCK_ACK_DISABLED_FOR_ALL_TID   (0)
+#define XRADIO_RX_BLOCK_ACK_DISABLED_FOR_ALL_TID   (0)
+
+#define XRADIO_BLOCK_ACK_CNT    (30)
+#define XRADIO_BLOCK_ACK_THLD   (800)
+#define XRADIO_BLOCK_ACK_HIST   (3)
+#define XRADIO_BLOCK_ACK_INTERVAL	(1 * HZ / XRADIO_BLOCK_ACK_HIST)
+#define XRWL_ALL_IFS           (-1)
+
+#ifdef ROAM_OFFLOAD
+#define XRADIO_SCAN_TYPE_ACTIVE 0x1000
+#define XRADIO_SCAN_BAND_5G     0x2000
+#endif /*ROAM_OFFLOAD*/
+
+#define IEEE80211_FCTL_WEP      0x4000
+#define IEEE80211_QOS_DATAGRP   0x0080
+
+#ifdef MCAST_FWDING
+#define WSM_MAX_BUF		30
+#endif
+
+#define MAX_RATES_STAGE   8   //
+#define MAX_RATES_RETRY   15
+
+#define XRADIO_WORKQUEUE   "xradio_wq"
+#define WIFI_CONF_PATH    "/data/xr_wifi.conf"
+
+/* extern */ struct task_struct;
+/* extern */ struct xradio_debug_priv;
+/* extern */ struct xradio_debug_common;
+/* extern */ struct firmware;
+
+/* Please keep order */
+enum xradio_join_status {
+	XRADIO_JOIN_STATUS_PASSIVE = 0,
+	XRADIO_JOIN_STATUS_MONITOR,
+	XRADIO_JOIN_STATUS_STA,
+	XRADIO_JOIN_STATUS_AP,
+};
+
+enum xradio_link_status {
+	XRADIO_LINK_OFF,
+	XRADIO_LINK_RESERVE,
+	XRADIO_LINK_SOFT,
+	XRADIO_LINK_HARD,
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+	XRADIO_LINK_RESET,
+	XRADIO_LINK_RESET_REMAP,
+#endif
+};
+
+enum xradio_bss_loss_status {
+	XRADIO_BSS_LOSS_NONE,
+	XRADIO_BSS_LOSS_CHECKING,
+	XRADIO_BSS_LOSS_CONFIRMING,
+	XRADIO_BSS_LOSS_CONFIRMED,
+};
+
+struct xradio_link_entry {
+	unsigned long			timestamp;
+	enum xradio_link_status		status;
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+	enum xradio_link_status		prev_status;
+#endif
+	u8				mac[ETH_ALEN];
+	u8				buffered[XRADIO_MAX_TID];
+	struct sk_buff_head		rx_queue;
+};
+
+#if defined(ROAM_OFFLOAD)
+struct xradio_testframe {
+	u8 len;
+	u8 *data;
+};
+#endif
+
+struct xradio_common {
+	struct xradio_debug_common	*debug;
+	struct xradio_queue		tx_queue[AC_QUEUE_NUM];
+	struct xradio_queue_stats	tx_queue_stats;
+
+	struct ieee80211_hw		*hw;
+	struct mac_address		addresses[XRWL_MAX_VIFS];
+
+	/*Will be a pointer to a list of VIFs - Dynamically allocated */
+	struct ieee80211_vif		*vif_list[XRWL_MAX_VIFS];
+	atomic_t			num_vifs;
+	spinlock_t			vif_list_lock;
+	u32				if_id_slot;
+	struct device			*pdev;
+	struct workqueue_struct		*workqueue;
+
+	struct mutex			conf_mutex;
+
+	struct sdio_func		*sdio_func;
+	int 			driver_ready;
+
+	/* HW/FW type (HIF_...) */
+	int				hw_type;
+	int				hw_revision;
+	int				fw_revision;
+
+	/* firmware/hardware info */
+	unsigned int tx_hdr_len;
+
+	/* Radio data */
+	int output_power;
+	int noise;
+
+	/* calibration, output power limit and rssi<->dBm conversation data */
+
+	/* BBP/MAC state */
+	const struct firmware		*sdd;
+	struct ieee80211_rate		*rates;
+	struct ieee80211_rate		*mcs_rates;
+	u8 mac_addr[ETH_ALEN];
+	/*TODO:COMBO: To be made per VIFF after mac80211 support */
+	struct ieee80211_channel	*channel;
+	int				channel_switch_in_progress;
+	wait_queue_head_t		channel_switch_done;
+	u8        channel_changed;   //add by yangfh 2015-5-15 16:57:38.
+	u8				long_frame_max_tx_count;
+	u8				short_frame_max_tx_count;
+	/* TODO:COMBO: According to Hong aggregation will happen per VIFF.
+	* Keeping in common structure for the time being. Will be moved to VIFF
+	* after the mechanism is clear */
+	u8				ba_tid_mask;
+	int				ba_acc; /*TODO: Same as above */
+	int				ba_cnt; /*TODO: Same as above */
+	int				ba_cnt_rx; /*TODO: Same as above */
+	int				ba_acc_rx; /*TODO: Same as above */
+	int				ba_hist; /*TODO: Same as above */
+	struct timer_list		ba_timer;/*TODO: Same as above */
+	spinlock_t			ba_lock; /*TODO: Same as above */
+	bool				ba_ena; /*TODO: Same as above */
+	struct work_struct              ba_work; /*TODO: Same as above */
+	struct xradio_pm_state		pm_state;
+	bool				is_BT_Present;
+	bool				is_go_thru_go_neg;
+	u8				conf_listen_interval;
+
+	/* BH */
+	atomic_t			bh_tx;
+	atomic_t			bh_term;
+	atomic_t			bh_suspend;
+	struct task_struct		*bh_thread;
+	int				bh_error;
+	wait_queue_head_t		bh_wq;
+	wait_queue_head_t		bh_evt_wq;
+
+
+	int				buf_id_tx;	/* byte */
+	int				buf_id_rx;	/* byte */
+	int				wsm_rx_seq;	/* byte */
+	int				wsm_tx_seq;	/* byte */
+	int				hw_bufs_used;
+	int				hw_bufs_used_vif[XRWL_MAX_VIFS];
+	struct sk_buff			*skb_cache;
+	struct sk_buff			*skb_reserved;
+	int						 skb_resv_len;
+	bool				powersave_enabled;
+	bool				device_can_sleep;
+	/* Keep xradio awake (WUP = 1) 1 second after each scan to avoid
+	 * FW issue with sleeping/waking up. */
+	atomic_t			recent_scan;
+	long			connet_time[XRWL_MAX_VIFS];
+#ifdef CONFIG_XRADIO_SUSPEND_POWER_OFF
+	atomic_t            suspend_state;
+#endif
+
+	/* WSM */
+	struct wsm_caps			wsm_caps;
+	struct mutex			wsm_cmd_mux;
+	struct wsm_buf			wsm_cmd_buf;
+	struct wsm_cmd			wsm_cmd;
+	wait_queue_head_t		wsm_cmd_wq;
+	wait_queue_head_t		wsm_startup_done;
+	struct semaphore		tx_lock_sem;
+	atomic_t				tx_lock;
+	u32				pending_frame_id;
+
+	/* WSM debug */
+	u32                 query_packetID;
+	atomic_t            query_cnt;
+	struct work_struct  query_work; /* for query packet */
+
+	/* Scan status */
+	struct xradio_scan scan;
+
+	/* TX/RX */
+	unsigned long		rx_timestamp;
+
+	/* WSM events */
+	spinlock_t		event_queue_lock;
+	struct list_head	event_queue;
+	struct work_struct	event_handler;
+
+	/* TX rate policy cache */
+	struct tx_policy_cache tx_policy_cache;
+	struct work_struct tx_policy_upload_work;
+	atomic_t upload_count;
+
+	/* cryptographic engine information */
+
+	/* bit field of glowing LEDs */
+	u16 softled_state;
+
+	/* statistics */
+	struct ieee80211_low_level_stats stats;
+
+	struct xradio_ht_oper		ht_oper;
+	int				tx_burst_idx;
+
+	struct ieee80211_iface_limit		if_limits1[2];
+	struct ieee80211_iface_limit		if_limits2[2];
+	struct ieee80211_iface_limit		if_limits3[2];
+	struct ieee80211_iface_combination	if_combs[3];
+
+	struct mutex			wsm_oper_lock;
+	struct delayed_work		rem_chan_timeout;
+	atomic_t			remain_on_channel;
+	int				roc_if_id;
+	u64				roc_cookie;
+	wait_queue_head_t		offchannel_wq;
+	u16				offchannel_done;
+	u16				prev_channel;
+	int       if_id_selected;
+	u32				key_map;
+	struct wsm_add_key		keys[WSM_KEY_MAX_INDEX + 1];
+#ifdef MCAST_FWDING
+	struct wsm_buf		wsm_release_buf[WSM_MAX_BUF];
+	u8			buf_released;
+#endif	
+#ifdef ROAM_OFFLOAD
+	u8				auto_scanning;
+	u8				frame_rcvd;
+	u8				num_scanchannels;
+	u8				num_2g_channels;
+	u8				num_5g_channels;
+	struct wsm_scan_ch		scan_channels[48];
+	struct sk_buff 			*beacon;
+	struct sk_buff 			*beacon_bkp;
+	struct xradio_testframe 	testframe;
+#endif /*ROAM_OFFLOAD*/
+
+	u8          connected_sta_cnt;
+	u16			vif0_throttle;
+	u16			vif1_throttle;
+};
+
+/* Virtual Interface State. One copy per VIF */
+struct xradio_vif {
+	atomic_t			enabled;
+	spinlock_t			vif_lock;
+	int				if_id;
+	/*TODO: Split into Common and VIF parts */
+	struct xradio_debug_priv	*debug;
+	/* BBP/MAC state */
+	u8 bssid[ETH_ALEN];
+	struct wsm_edca_params		edca;
+	struct wsm_tx_queue_params	tx_queue_params;
+	struct wsm_association_mode	association_mode;
+	struct wsm_set_bss_params	bss_params;
+	struct wsm_set_pm		powersave_mode;
+	struct wsm_set_pm		firmware_ps_mode;
+	int				power_set_true;
+	int				user_power_set_true;
+	u8				user_pm_mode;
+	int				cqm_rssi_thold;
+	unsigned			cqm_rssi_hyst;
+	unsigned			cqm_tx_failure_thold;
+	unsigned			cqm_tx_failure_count;
+	bool				cqm_use_rssi;
+	int				cqm_link_loss_count;
+	int				cqm_beacon_loss_count;
+	int				mode;
+	bool				enable_beacon;
+	int				beacon_int;
+	size_t				ssid_length;
+	u8				ssid[IEEE80211_MAX_SSID_LEN];
+#ifdef HIDDEN_SSID
+	bool				hidden_ssid;
+#endif
+	bool				listening;
+	struct wsm_rx_filter		rx_filter;
+	struct wsm_beacon_filter_table	bf_table;
+	struct wsm_beacon_filter_control bf_control;
+	struct wsm_multicast_filter	multicast_filter;
+	bool				has_multicast_subscription;
+	struct wsm_broadcast_addr_filter	broadcast_filter;
+	bool				disable_beacon_filter;
+	struct wsm_arp_ipv4_filter      filter4;
+	struct work_struct		update_filtering_work;
+	struct work_struct		set_beacon_wakeup_period_work;
+	struct xradio_pm_state_vif	pm_state_vif;
+	/*TODO: Add support in mac80211 for psmode info per VIF */
+	struct wsm_p2p_ps_modeinfo	p2p_ps_modeinfo;
+	struct wsm_uapsd_info		uapsd_info;
+	bool				setbssparams_done;
+	u32				listen_interval;
+	u32				erp_info;
+	bool				powersave_enabled;
+
+	/* WSM Join */
+	enum xradio_join_status	join_status;
+	u8			join_bssid[ETH_ALEN];
+	struct work_struct	join_work;
+	struct delayed_work	join_timeout;
+	struct work_struct	unjoin_work;
+	struct work_struct	offchannel_work;
+	int			join_dtim_period;
+	atomic_t	delayed_unjoin;
+
+	/* Security */
+	s8			wep_default_key_id;
+	struct work_struct	wep_key_work;
+        unsigned long           rx_timestamp;
+        u32                     cipherType;
+
+
+	/* AP powersave */
+	u32			link_id_map;
+	u32			max_sta_ap_mode;
+	u32			link_id_after_dtim;
+	u32			link_id_uapsd;
+	u32			link_id_max;
+	u32			wsm_key_max_idx;
+	struct xradio_link_entry link_id_db[MAX_STA_IN_AP_MODE];
+	struct work_struct	link_id_work;
+	struct delayed_work	link_id_gc_work;
+	u32			sta_asleep_mask;
+	u32			pspoll_mask;
+	bool			aid0_bit_set;
+	spinlock_t		ps_state_lock;
+	bool			buffered_multicasts;
+	bool			tx_multicast;
+	u8     last_tim[8];   //for softap dtim, add by yangfh
+	struct work_struct	set_tim_work;
+	struct delayed_work	set_cts_work;
+	struct work_struct	multicast_start_work;
+	struct work_struct	multicast_stop_work;
+	struct timer_list	mcast_timeout;
+
+	/* CQM Implementation */
+	struct delayed_work	bss_loss_work;
+	struct delayed_work	connection_loss_work;
+	struct work_struct	tx_failure_work;
+	int			delayed_link_loss;
+	spinlock_t		bss_loss_lock;
+	int			bss_loss_status;
+	int			bss_loss_confirm_id;
+
+	struct ieee80211_vif	*vif;
+	struct xradio_common	*hw_priv;
+	struct ieee80211_hw	*hw;
+
+	/* ROC implementation */
+	struct delayed_work		pending_offchanneltx_work;
+#if defined(CONFIG_XRADIO_USE_EXTENSIONS)
+	/* Workaround for WFD testcase 6.1.10*/
+	struct work_struct	linkid_reset_work;
+	u8			action_frame_sa[ETH_ALEN];
+	u8			action_linkid;
+#endif
+	bool			htcap;
+#ifdef  AP_HT_CAP_UPDATE
+        u16                     ht_oper;
+        struct work_struct      ht_oper_update_work;
+#endif
+
+#ifdef AP_HT_COMPAT_FIX
+	u16    ht_compat_cnt;
+	u16    ht_compat_det;
+#endif
+};
+struct xradio_sta_priv {
+	int link_id;
+	struct xradio_vif *priv;
+};
+enum xradio_data_filterid {
+	IPV4ADDR_FILTER_ID = 0,
+};
+
+/* Datastructure for LLC-SNAP HDR */
+#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 */
+} __packed;
+
+
+#ifdef TES_P2P_0002_ROC_RESTART
+extern s32  TES_P2P_0002_roc_dur;
+extern s32  TES_P2P_0002_roc_sec;
+extern s32  TES_P2P_0002_roc_usec;
+extern u32  TES_P2P_0002_packet_id;
+extern u32  TES_P2P_0002_state;
+
+#define TES_P2P_0002_STATE_IDLE       0x00
+#define TES_P2P_0002_STATE_SEND_RESP  0x01
+#define TES_P2P_0002_STATE_GET_PKTID  0x02
+#endif
+
+/* debug.h must be here because refer to struct xradio_vif and 
+   struct xradio_common.*/
+#include "debug.h"
+
+/*******************************************************
+ interfaces for operations of vif.
+********************************************************/
+static inline
+struct xradio_common *xrwl_vifpriv_to_hwpriv(struct xradio_vif *priv)
+{
+	return priv->hw_priv;
+}
+static inline
+struct xradio_vif *xrwl_get_vif_from_ieee80211(struct ieee80211_vif *vif)
+{
+	return  (struct xradio_vif *)vif->drv_priv;
+}
+
+static inline
+struct xradio_vif *xrwl_hwpriv_to_vifpriv(struct xradio_common *hw_priv,
+						int if_id)
+{
+	struct xradio_vif *vif;
+
+	if (WARN_ON((-1 == if_id) || (if_id > XRWL_MAX_VIFS)))
+		return NULL;
+	/* TODO:COMBO: During scanning frames can be received
+	 * on interface ID 3 */
+	spin_lock(&hw_priv->vif_list_lock);
+	if (!hw_priv->vif_list[if_id]) {
+		spin_unlock(&hw_priv->vif_list_lock);
+		return NULL;
+	}
+
+	vif = xrwl_get_vif_from_ieee80211(hw_priv->vif_list[if_id]);
+	WARN_ON(!vif);
+	if (vif)
+		spin_lock(&vif->vif_lock);
+	spin_unlock(&hw_priv->vif_list_lock);
+	return vif;
+}
+
+static inline
+struct xradio_vif *__xrwl_hwpriv_to_vifpriv(struct xradio_common *hw_priv,
+					      int if_id)
+{
+	WARN_ON((-1 == if_id) || (if_id > XRWL_MAX_VIFS));
+	/* TODO:COMBO: During scanning frames can be received
+	 * on interface ID 3 */
+	if (!hw_priv->vif_list[if_id]) {
+		return NULL;
+	}
+
+	return xrwl_get_vif_from_ieee80211(hw_priv->vif_list[if_id]);
+}
+
+static inline
+struct xradio_vif *xrwl_get_activevif(struct xradio_common *hw_priv)
+{
+	return xrwl_hwpriv_to_vifpriv(hw_priv, ffs(hw_priv->if_id_slot)-1);
+}
+
+static inline bool is_hardware_xradio(struct xradio_common *hw_priv)
+{
+	return (hw_priv->hw_revision == XR819_HW_REV0);
+}
+
+static inline int xrwl_get_nr_hw_ifaces(struct xradio_common *hw_priv)
+{
+	switch(hw_priv->hw_revision) {
+		case XR819_HW_REV0:
+		default:
+			return 1;
+	}
+}
+
+#define xradio_for_each_vif(_hw_priv, _priv, _i)			\
+	for(		\
+		_i = 0; 							 \
+		(_i < XRWL_MAX_VIFS)				  \
+		&& ((_priv = _hw_priv->vif_list[_i] ?			 \
+		xrwl_get_vif_from_ieee80211(_hw_priv->vif_list[_i]) : NULL),1);	 \
+		_i++		 \
+	)
+
+/*******************************************************
+ interfaces for operations of queue.
+********************************************************/
+static inline void xradio_tx_queues_lock(struct xradio_common *hw_priv)
+{
+	int i;
+	for (i = 0; i < 4; ++i)
+		xradio_queue_lock(&hw_priv->tx_queue[i]);
+}
+
+static inline void xradio_tx_queues_unlock(struct xradio_common *hw_priv)
+{
+	int i;
+	for (i = 0; i < 4; ++i)
+		xradio_queue_unlock(&hw_priv->tx_queue[i]);
+}
+
+#endif /* XRADIO_H */
