Updating libertas wireless driver to latest version.

SVN-Revision: 10235

22 files changed, 1151 insertions, 1231 deletions

diff --git a/package/libertas/src/11d.c b/package/libertas/src/11d.c
index 5e10ce0d35..40f1daadb5 100644
--- a/package/libertas/src/11d.c
+++ b/package/libertas/src/11d.c
@@ -46,11 +46,13 @@ static struct chan_freq_power channel_freq_power_UN_BG[] = {
 static u8 lbs_region_2_code(u8 *region)
 {
 	u8 i;
+	u8 size = sizeof(region_code_mapping)/
+		  sizeof(struct region_code_mapping);
 
 	for (i = 0; region[i] && i < COUNTRY_CODE_LEN; i++)
 		region[i] = toupper(region[i]);
 
-	for (i = 0; i < ARRAY_SIZE(region_code_mapping); i++) {
+	for (i = 0; i < size; i++) {
 		if (!memcmp(region, region_code_mapping[i].region,
 			    COUNTRY_CODE_LEN))
 			return (region_code_mapping[i].code);
@@ -63,8 +65,9 @@ static u8 lbs_region_2_code(u8 *region)
 static u8 *lbs_code_2_region(u8 code)
 {
 	u8 i;
-
-	for (i = 0; i < ARRAY_SIZE(region_code_mapping); i++) {
+	u8 size = sizeof(region_code_mapping)
+		  / sizeof(struct region_code_mapping);
+	for (i = 0; i < size; i++) {
 		if (region_code_mapping[i].code == code)
 			return (region_code_mapping[i].region);
 	}
@@ -87,7 +90,8 @@ static u8 lbs_get_chan_11d(u8 band, u8 firstchan, u8 nrchan, u8 *chan)
 	u8 cfp_no;
 
 	cfp = channel_freq_power_UN_BG;
-	cfp_no = ARRAY_SIZE(channel_freq_power_UN_BG);
+	cfp_no = sizeof(channel_freq_power_UN_BG) /
+	    sizeof(struct chan_freq_power);
 
 	for (i = 0; i < cfp_no; i++) {
 		if ((cfp + i)->channel == firstchan) {
@@ -137,12 +141,16 @@ static u8 lbs_channel_known_11d(u8 chan,
 u32 lbs_chan_2_freq(u8 chan, u8 band)
 {
 	struct chan_freq_power *cf;
+	u16 cnt;
 	u16 i;
 	u32 freq = 0;
 
 	cf = channel_freq_power_UN_BG;
+	cnt =
+	    sizeof(channel_freq_power_UN_BG) /
+	    sizeof(struct chan_freq_power);
 
-	for (i = 0; i < ARRAY_SIZE(channel_freq_power_UN_BG); i++) {
+	for (i = 0; i < cnt; i++) {
 		if (chan == cf[i].channel)
 			freq = cf[i].freq;
 	}
diff --git a/package/libertas/src/:0 b/package/libertas/src/:0
deleted file mode 100644
index ff68859c72..0000000000
--- a/package/libertas/src/:0
+++ /dev/null
@@ -1,62 +0,0 @@
-# 
-# Copyright (C) 2007 OpenWrt.org
-#
-# This is free software, licensed under the GNU General Public License v2.
-# See /LICENSE for more information.
-#
-# $Id: Makefile 8694 2007-09-08 19:55:42Z nbd $
-
-include $(TOPDIR)/rules.mk
-include $(INCLUDE_DIR)/kernel.mk
-
-PKG_NAME:=kmod-libertas
-PKG_RELEASE:=1
-
-include $(INCLUDE_DIR)/package.mk
-
-define KernelPackage/libertas
-  SUBMENU:=Other modules
-  DEPENDS:=@TARGET_olpc +kmod-mac80211
-  TITLE:=Marvell 88W8015 Wireless Driver
-  FILES:= \
-	$(PKG_BUILD_DIR)/libertas.$(LINUX_KMOD_SUFFIX)  \
-	$(PKG_BUILD_DIR)/usb8xxx.$(LINUX_KMOD_SUFFIX) 
-#  AUTOLOAD:=$(call AutoLoad,20,switch-core switch-robo switch-adm)
-endef
-
-define Download/firmware
-  URL:=http://dev.laptop.org/pub/firmware/libertas
-  FILE:=usb8388-5.220.11.p5.bin
-  MD5SUM=123
-endef
-
-define Download/firmware_license
-  URL:=http://dev.laptop.org/pub/firmware/libertas
-  FILE:=LICENSE
-  MD5SUM=123
-endef
-
-define Build/Prepare
-	mkdir -p $(PKG_BUILD_DIR)
-	$(CP) ./src/* $(PKG_BUILD_DIR)/
-endef
-
-define Build/Compile
-	$(MAKE) -C "$(LINUX_DIR)" \
-		CROSS_COMPILE="$(TARGET_CROSS)" \
-		ARCH="$(LINUX_KARCH)" \
-		SUBDIRS="$(PKG_BUILD_DIR)" \
-		CONFIG_LIBERTAS=m \
-		CONFIG_LIBERTAS_USB=m \
-		EXTRA_CFLAGS="-I$(PKG_BUILD_DIR) -include compat.h -I$(STAGING_DIR)/usr/include/mac80211" \
-		modules 
-endef
-
-define KernelPackage/libertas/install
-	$(INSTALL_DIR) $(1)/lib/firmware
-	$(INSTALL_BIN) $(DL_DIR)/usb8388-5.220.11.p5.bin $(1)/lib/firmware/usb8388.bin
-	$(INSTALL_BIN) $(DL_DIR)/LICENSE $(1)/lib/firmware/
-endef
-
-$(eval $(call KernelPackage,libertas))
-$(eval $(call Download,firmware))
diff --git a/package/libertas/src/assoc.c b/package/libertas/src/assoc.c
index 7b672fe62c..c622e9b63c 100644
--- a/package/libertas/src/assoc.c
+++ b/package/libertas/src/assoc.c
@@ -163,17 +163,18 @@ done:
 }
 
 
-static int update_channel(struct lbs_private *priv)
+int lbs_update_channel(struct lbs_private *priv)
 {
 	int ret;
 
-	/* the channel in f/w could be out of sync, get the current channel */
+	/* the channel in f/w could be out of sync; get the current channel */
 	lbs_deb_enter(LBS_DEB_ASSOC);
 
 	ret = lbs_get_channel(priv);
-	if (ret > 0)
-		priv->curbssparams.channel = (u8) ret;
-
+	if (ret > 0) {
+		priv->curbssparams.channel = ret;
+		ret = 0;
+	}
 	lbs_deb_leave_args(LBS_DEB_ASSOC, "ret %d", ret);
 	return ret;
 }
@@ -184,7 +185,7 @@ void lbs_sync_channel(struct work_struct *work)
 		sync_channel);
 
 	lbs_deb_enter(LBS_DEB_ASSOC);
-	if (update_channel(priv) != 0)
+	if (lbs_update_channel(priv))
 		lbs_pr_info("Channel synchronization failed.");
 	lbs_deb_leave(LBS_DEB_ASSOC);
 }
@@ -196,33 +197,37 @@ static int assoc_helper_channel(struct lbs_private *priv,
 
 	lbs_deb_enter(LBS_DEB_ASSOC);
 
-	ret = update_channel(priv);
-	if (ret < 0) {
-		lbs_deb_assoc("ASSOC: channel: error getting channel.");
+	ret = lbs_update_channel(priv);
+	if (ret) {
+		lbs_deb_assoc("ASSOC: channel: error getting channel.\n");
+		goto done;
 	}
 
 	if (assoc_req->channel == priv->curbssparams.channel)
 		goto done;
 
 	if (priv->mesh_dev) {
-		/* Disconnect mesh while associating -- otherwise it
-		   won't let us change channels */
-		lbs_mesh_config(priv, 0);
+		/* Change mesh channel first; 21.p21 firmware won't let
+		   you change channel otherwise (even though it'll return
+		   an error to this */
+		lbs_mesh_config(priv, 0, assoc_req->channel);
 	}
 
 	lbs_deb_assoc("ASSOC: channel: %d -> %d\n",
-	       priv->curbssparams.channel, assoc_req->channel);
+		      priv->curbssparams.channel, assoc_req->channel);
 
 	ret = lbs_set_channel(priv, assoc_req->channel);
 	if (ret < 0)
-		lbs_deb_assoc("ASSOC: channel: error setting channel.");
+		lbs_deb_assoc("ASSOC: channel: error setting channel.\n");
 
 	/* FIXME: shouldn't need to grab the channel _again_ after setting
 	 * it since the firmware is supposed to return the new channel, but
 	 * whatever... */
-	ret = update_channel(priv);
-	if (ret < 0)
-		lbs_deb_assoc("ASSOC: channel: error getting channel.");
+	ret = lbs_update_channel(priv);
+	if (ret) {
+		lbs_deb_assoc("ASSOC: channel: error getting channel.\n");
+		goto done;
+	}
 
 	if (assoc_req->channel != priv->curbssparams.channel) {
 		lbs_deb_assoc("ASSOC: channel: failed to update channel to %d\n",
@@ -240,11 +245,11 @@ static int assoc_helper_channel(struct lbs_private *priv,
 	}
 
 	/* Must restart/rejoin adhoc networks after channel change */
-	set_bit(ASSOC_FLAG_SSID, &assoc_req->flags);
+ 	set_bit(ASSOC_FLAG_SSID, &assoc_req->flags);
 
  restore_mesh:
 	if (priv->mesh_dev)
-		lbs_mesh_config(priv, 1);
+		lbs_mesh_config(priv, 1, priv->curbssparams.channel);
 
  done:
 	lbs_deb_leave_args(LBS_DEB_ASSOC, "ret %d", ret);
@@ -253,7 +258,7 @@ static int assoc_helper_channel(struct lbs_private *priv,
 
 
 static int assoc_helper_wep_keys(struct lbs_private *priv,
-                                 struct assoc_request * assoc_req)
+				 struct assoc_request *assoc_req)
 {
 	int i;
 	int ret = 0;
@@ -261,22 +266,11 @@ static int assoc_helper_wep_keys(struct lbs_private *priv,
 	lbs_deb_enter(LBS_DEB_ASSOC);
 
 	/* Set or remove WEP keys */
-	if (   assoc_req->wep_keys[0].len
-	    || assoc_req->wep_keys[1].len
-	    || assoc_req->wep_keys[2].len
-	    || assoc_req->wep_keys[3].len) {
-		ret = lbs_prepare_and_send_command(priv,
-					    CMD_802_11_SET_WEP,
-					    CMD_ACT_ADD,
-					    CMD_OPTION_WAITFORRSP,
-					    0, assoc_req);
-	} else {
-		ret = lbs_prepare_and_send_command(priv,
-					    CMD_802_11_SET_WEP,
-					    CMD_ACT_REMOVE,
-					    CMD_OPTION_WAITFORRSP,
-					    0, NULL);
-	}
+	if (assoc_req->wep_keys[0].len || assoc_req->wep_keys[1].len ||
+	    assoc_req->wep_keys[2].len || assoc_req->wep_keys[3].len)
+		ret = lbs_cmd_802_11_set_wep(priv, CMD_ACT_ADD, assoc_req);
+	else
+		ret = lbs_cmd_802_11_set_wep(priv, CMD_ACT_REMOVE, assoc_req);
 
 	if (ret)
 		goto out;
@@ -286,6 +280,7 @@ static int assoc_helper_wep_keys(struct lbs_private *priv,
 		priv->currentpacketfilter |= CMD_ACT_MAC_WEP_ENABLE;
 	else
 		priv->currentpacketfilter &= ~CMD_ACT_MAC_WEP_ENABLE;
+
 	ret = lbs_set_mac_packet_filter(priv);
 	if (ret)
 		goto out;
@@ -295,7 +290,7 @@ static int assoc_helper_wep_keys(struct lbs_private *priv,
 	/* Copy WEP keys into priv wep key fields */
 	for (i = 0; i < 4; i++) {
 		memcpy(&priv->wep_keys[i], &assoc_req->wep_keys[i],
-			sizeof(struct enc_key));
+		       sizeof(struct enc_key));
 	}
 	priv->wep_tx_keyidx = assoc_req->wep_tx_keyidx;
 
@@ -310,8 +305,8 @@ static int assoc_helper_secinfo(struct lbs_private *priv,
                                 struct assoc_request * assoc_req)
 {
 	int ret = 0;
-	u32 do_wpa;
-	u32 rsn = 0;
+	uint16_t do_wpa;
+	uint16_t rsn = 0;
 
 	lbs_deb_enter(LBS_DEB_ASSOC);
 
@@ -328,28 +323,19 @@ static int assoc_helper_secinfo(struct lbs_private *priv,
 	 */
 
 	/* Get RSN enabled/disabled */
-	ret = lbs_prepare_and_send_command(priv,
-				    CMD_802_11_ENABLE_RSN,
-				    CMD_ACT_GET,
-				    CMD_OPTION_WAITFORRSP,
-				    0, &rsn);
+	ret = lbs_cmd_802_11_enable_rsn(priv, CMD_ACT_GET, &rsn);
 	if (ret) {
-		lbs_deb_assoc("Failed to get RSN status: %d", ret);
+		lbs_deb_assoc("Failed to get RSN status: %d\n", ret);
 		goto out;
 	}
 
 	/* Don't re-enable RSN if it's already enabled */
-	do_wpa = (assoc_req->secinfo.WPAenabled || assoc_req->secinfo.WPA2enabled);
+	do_wpa = assoc_req->secinfo.WPAenabled || assoc_req->secinfo.WPA2enabled;
 	if (do_wpa == rsn)
 		goto out;
 
 	/* Set RSN enabled/disabled */
-	rsn = do_wpa;
-	ret = lbs_prepare_and_send_command(priv,
-				    CMD_802_11_ENABLE_RSN,
-				    CMD_ACT_SET,
-				    CMD_OPTION_WAITFORRSP,
-				    0, &rsn);
+	ret = lbs_cmd_802_11_enable_rsn(priv, CMD_ACT_SET, &do_wpa);
 
 out:
 	lbs_deb_leave_args(LBS_DEB_ASSOC, "ret %d", ret);
diff --git a/package/libertas/src/cmd.c b/package/libertas/src/cmd.c
index 01d23493b4..4d06dec6cd 100644
--- a/package/libertas/src/cmd.c
+++ b/package/libertas/src/cmd.c
@@ -13,11 +13,10 @@
 #include "wext.h"
 #include "cmd.h"
 
-static void cleanup_cmdnode(struct cmd_ctrl_node *ptempnode);
 static struct cmd_ctrl_node *lbs_get_cmd_ctrl_node(struct lbs_private *priv);
 static void lbs_set_cmd_ctrl_node(struct lbs_private *priv,
 		    struct cmd_ctrl_node *ptempnode,
-		    u16 wait_option, void *pdata_buf);
+		    void *pdata_buf);
 
 
 /**
@@ -56,7 +55,7 @@ int lbs_update_hw_spec(struct lbs_private *priv)
 	memset(&cmd, 0, sizeof(cmd));
 	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 	memcpy(cmd.permanentaddr, priv->current_addr, ETH_ALEN);
-	ret = lbs_cmd_with_response(priv, CMD_GET_HW_SPEC, cmd);
+	ret = lbs_cmd_with_response(priv, CMD_GET_HW_SPEC, &cmd);
 	if (ret)
 		goto out;
 
@@ -111,6 +110,28 @@ out:
 	return ret;
 }
 
+int lbs_host_sleep_cfg(struct lbs_private *priv, uint32_t criteria)
+{
+	struct cmd_ds_host_sleep cmd_config;
+	int ret;
+
+	cmd_config.hdr.size = cpu_to_le16(sizeof(cmd_config));
+	cmd_config.criteria = cpu_to_le32(criteria);
+	cmd_config.gpio = priv->wol_gpio;
+	cmd_config.gap = priv->wol_gap;
+
+	ret = lbs_cmd_with_response(priv, CMD_802_11_HOST_SLEEP_CFG, &cmd_config);
+	if (!ret) {
+		lbs_deb_cmd("Set WOL criteria to %x\n", criteria);
+		priv->wol_criteria = criteria;
+	} else {
+		lbs_pr_info("HOST_SLEEP_CFG failed %d\n", ret);
+	}
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(lbs_host_sleep_cfg);
+
 static int lbs_cmd_802_11_ps_mode(struct lbs_private *priv,
 				   struct cmd_ds_command *cmd,
 				   u16 cmd_action)
@@ -150,204 +171,160 @@ static int lbs_cmd_802_11_ps_mode(struct lbs_private *priv,
 	return 0;
 }
 
-static int lbs_cmd_802_11_inactivity_timeout(struct lbs_private *priv,
-					      struct cmd_ds_command *cmd,
-					      u16 cmd_action, void *pdata_buf)
+int lbs_cmd_802_11_inactivity_timeout(struct lbs_private *priv,
+				      uint16_t cmd_action, uint16_t *timeout)
 {
-	u16 *timeout = pdata_buf;
+	struct cmd_ds_802_11_inactivity_timeout cmd;
+	int ret;
 
 	lbs_deb_enter(LBS_DEB_CMD);
 
-	cmd->command = cpu_to_le16(CMD_802_11_INACTIVITY_TIMEOUT);
-	cmd->size =
-	    cpu_to_le16(sizeof(struct cmd_ds_802_11_inactivity_timeout)
-			     + S_DS_GEN);
+	cmd.hdr.command = cpu_to_le16(CMD_802_11_INACTIVITY_TIMEOUT);
+	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 
-	cmd->params.inactivity_timeout.action = cpu_to_le16(cmd_action);
+	cmd.action = cpu_to_le16(cmd_action);
 
-	if (cmd_action)
-		cmd->params.inactivity_timeout.timeout = cpu_to_le16(*timeout);
+	if (cmd_action == CMD_ACT_SET)
+		cmd.timeout = cpu_to_le16(*timeout);
 	else
-		cmd->params.inactivity_timeout.timeout = 0;
+		cmd.timeout = 0;
 
-	lbs_deb_leave(LBS_DEB_CMD);
+	ret = lbs_cmd_with_response(priv, CMD_802_11_INACTIVITY_TIMEOUT, &cmd);
+
+	if (!ret)
+		*timeout = le16_to_cpu(cmd.timeout);
+
+	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 	return 0;
 }
 
-static int lbs_cmd_802_11_sleep_params(struct lbs_private *priv,
-					struct cmd_ds_command *cmd,
-					u16 cmd_action)
+int lbs_cmd_802_11_sleep_params(struct lbs_private *priv, uint16_t cmd_action,
+				struct sleep_params *sp)
 {
-	struct cmd_ds_802_11_sleep_params *sp = &cmd->params.sleep_params;
+	struct cmd_ds_802_11_sleep_params cmd;
+	int ret;
 
 	lbs_deb_enter(LBS_DEB_CMD);
 
-	cmd->size = cpu_to_le16((sizeof(struct cmd_ds_802_11_sleep_params)) +
-				S_DS_GEN);
-	cmd->command = cpu_to_le16(CMD_802_11_SLEEP_PARAMS);
-
 	if (cmd_action == CMD_ACT_GET) {
-		memset(&priv->sp, 0, sizeof(struct sleep_params));
-		memset(sp, 0, sizeof(struct cmd_ds_802_11_sleep_params));
-		sp->action = cpu_to_le16(cmd_action);
-	} else if (cmd_action == CMD_ACT_SET) {
-		sp->action = cpu_to_le16(cmd_action);
-		sp->error = cpu_to_le16(priv->sp.sp_error);
-		sp->offset = cpu_to_le16(priv->sp.sp_offset);
-		sp->stabletime = cpu_to_le16(priv->sp.sp_stabletime);
-		sp->calcontrol = (u8) priv->sp.sp_calcontrol;
-		sp->externalsleepclk = (u8) priv->sp.sp_extsleepclk;
-		sp->reserved = cpu_to_le16(priv->sp.sp_reserved);
+		memset(&cmd, 0, sizeof(cmd));
+	} else {
+		cmd.error = cpu_to_le16(sp->sp_error);
+		cmd.offset = cpu_to_le16(sp->sp_offset);
+		cmd.stabletime = cpu_to_le16(sp->sp_stabletime);
+		cmd.calcontrol = sp->sp_calcontrol;
+		cmd.externalsleepclk = sp->sp_extsleepclk;
+		cmd.reserved = cpu_to_le16(sp->sp_reserved);
+	}
+	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
+	cmd.action = cpu_to_le16(cmd_action);
+
+	ret = lbs_cmd_with_response(priv, CMD_802_11_SLEEP_PARAMS, &cmd);
+
+	if (!ret) {
+		lbs_deb_cmd("error 0x%x, offset 0x%x, stabletime 0x%x, "
+			    "calcontrol 0x%x extsleepclk 0x%x\n",
+			    le16_to_cpu(cmd.error), le16_to_cpu(cmd.offset),
+			    le16_to_cpu(cmd.stabletime), cmd.calcontrol,
+			    cmd.externalsleepclk);
+
+		sp->sp_error = le16_to_cpu(cmd.error);
+		sp->sp_offset = le16_to_cpu(cmd.offset);
+		sp->sp_stabletime = le16_to_cpu(cmd.stabletime);
+		sp->sp_calcontrol = cmd.calcontrol;
+		sp->sp_extsleepclk = cmd.externalsleepclk;
+		sp->sp_reserved = le16_to_cpu(cmd.reserved);
 	}
 
-	lbs_deb_leave(LBS_DEB_CMD);
+	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 	return 0;
 }
 
-static int lbs_cmd_802_11_set_wep(struct lbs_private *priv,
-                                   struct cmd_ds_command *cmd,
-                                   u32 cmd_act,
-                                   void * pdata_buf)
+int lbs_cmd_802_11_set_wep(struct lbs_private *priv, uint16_t cmd_action,
+			   struct assoc_request *assoc)
 {
-	struct cmd_ds_802_11_set_wep *wep = &cmd->params.wep;
+	struct cmd_ds_802_11_set_wep cmd;
 	int ret = 0;
-	struct assoc_request * assoc_req = pdata_buf;
 
 	lbs_deb_enter(LBS_DEB_CMD);
 
-	cmd->command = cpu_to_le16(CMD_802_11_SET_WEP);
-	cmd->size = cpu_to_le16(sizeof(*wep) + S_DS_GEN);
+	cmd.hdr.command = cpu_to_le16(CMD_802_11_SET_WEP);
+	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 
-	if (cmd_act == CMD_ACT_ADD) {
-		int i;
+	cmd.action = cpu_to_le16(cmd_action);
 
-		if (!assoc_req) {
-			lbs_deb_cmd("Invalid association request!");
-			ret = -1;
-			goto done;
-		}
-
-		wep->action = cpu_to_le16(CMD_ACT_ADD);
+	if (cmd_action == CMD_ACT_ADD) {
+		int i;
 
 		/* default tx key index */
-		wep->keyindex = cpu_to_le16((u16)(assoc_req->wep_tx_keyidx &
-						  (u32)CMD_WEP_KEY_INDEX_MASK));
+		cmd.keyindex = cpu_to_le16(assoc->wep_tx_keyidx &
+					   CMD_WEP_KEY_INDEX_MASK);
 
 		/* Copy key types and material to host command structure */
 		for (i = 0; i < 4; i++) {
-			struct enc_key * pkey = &assoc_req->wep_keys[i];
+			struct enc_key *pkey = &assoc->wep_keys[i];
 
 			switch (pkey->len) {
 			case KEY_LEN_WEP_40:
-				wep->keytype[i] = CMD_TYPE_WEP_40_BIT;
-				memmove(&wep->keymaterial[i], pkey->key,
-				        pkey->len);
+				cmd.keytype[i] = CMD_TYPE_WEP_40_BIT;
+				memmove(cmd.keymaterial[i], pkey->key, pkey->len);
 				lbs_deb_cmd("SET_WEP: add key %d (40 bit)\n", i);
 				break;
 			case KEY_LEN_WEP_104:
-				wep->keytype[i] = CMD_TYPE_WEP_104_BIT;
-				memmove(&wep->keymaterial[i], pkey->key,
-				        pkey->len);
+				cmd.keytype[i] = CMD_TYPE_WEP_104_BIT;
+				memmove(cmd.keymaterial[i], pkey->key, pkey->len);
 				lbs_deb_cmd("SET_WEP: add key %d (104 bit)\n", i);
 				break;
 			case 0:
 				break;
 			default:
 				lbs_deb_cmd("SET_WEP: invalid key %d, length %d\n",
-				       i, pkey->len);
+					    i, pkey->len);
 				ret = -1;
 				goto done;
 				break;
 			}
 		}
-	} else if (cmd_act == CMD_ACT_REMOVE) {
+	} else if (cmd_action == CMD_ACT_REMOVE) {
 		/* ACT_REMOVE clears _all_ WEP keys */
-		wep->action = cpu_to_le16(CMD_ACT_REMOVE);
 
 		/* default tx key index */
-		wep->keyindex = cpu_to_le16((u16)(priv->wep_tx_keyidx &
-						  (u32)CMD_WEP_KEY_INDEX_MASK));
+		cmd.keyindex = cpu_to_le16(priv->wep_tx_keyidx &
+					   CMD_WEP_KEY_INDEX_MASK);
 		lbs_deb_cmd("SET_WEP: remove key %d\n", priv->wep_tx_keyidx);
 	}
 
-	ret = 0;
-
+	ret = lbs_cmd_with_response(priv, CMD_802_11_SET_WEP, &cmd);
 done:
 	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 	return ret;
 }
 
-static int lbs_cmd_802_11_enable_rsn(struct lbs_private *priv,
-				      struct cmd_ds_command *cmd,
-				      u16 cmd_action,
-				      void * pdata_buf)
+int lbs_cmd_802_11_enable_rsn(struct lbs_private *priv, uint16_t cmd_action,
+			      uint16_t *enable)
 {
-	struct cmd_ds_802_11_enable_rsn *penableRSN = &cmd->params.enbrsn;
-	u32 * enable = pdata_buf;
+	struct cmd_ds_802_11_enable_rsn cmd;
+	int ret;
 
 	lbs_deb_enter(LBS_DEB_CMD);
 
-	cmd->command = cpu_to_le16(CMD_802_11_ENABLE_RSN);
-	cmd->size = cpu_to_le16(sizeof(*penableRSN) + S_DS_GEN);
-	penableRSN->action = cpu_to_le16(cmd_action);
+	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
+	cmd.action = cpu_to_le16(cmd_action);
 
 	if (cmd_action == CMD_ACT_SET) {
 		if (*enable)
-			penableRSN->enable = cpu_to_le16(CMD_ENABLE_RSN);
+			cmd.enable = cpu_to_le16(CMD_ENABLE_RSN);
 		else
-			penableRSN->enable = cpu_to_le16(CMD_DISABLE_RSN);
+			cmd.enable = cpu_to_le16(CMD_DISABLE_RSN);
 		lbs_deb_cmd("ENABLE_RSN: %d\n", *enable);
 	}
 
-	lbs_deb_leave(LBS_DEB_CMD);
-	return 0;
-}
-
-
-static ssize_t lbs_tlv_size(const u8 *tlv, u16 size)
-{
-	ssize_t pos = 0;
-	struct mrvlietypesheader *tlv_h;
-	while (pos < size) {
-		u16 length;
-		tlv_h = (struct mrvlietypesheader *) tlv;
-		if (tlv_h->len == 0)
-			return pos;
-		length = le16_to_cpu(tlv_h->len) +
-			sizeof(struct mrvlietypesheader);
-		pos += length;
-		tlv += length;
-	}
-	return pos;
-}
-
-
-static void lbs_cmd_802_11_subscribe_event(struct lbs_private *priv,
-	struct cmd_ds_command *cmd, u16 cmd_action,
-	void *pdata_buf)
-{
-	struct cmd_ds_802_11_subscribe_event *events =
-		(struct cmd_ds_802_11_subscribe_event *) pdata_buf;
-
-	/* pdata_buf points to a struct cmd_ds_802_11_subscribe_event and room
-	 * for various Marvell TLVs */
-
-	lbs_deb_enter(LBS_DEB_CMD);
-
-	cmd->size = cpu_to_le16(sizeof(*events)
-			- sizeof(events->tlv)
-			+ S_DS_GEN);
-	cmd->params.subscribe_event.action = cpu_to_le16(cmd_action);
-	if (cmd_action == CMD_ACT_GET) {
-		cmd->params.subscribe_event.events = 0;
-	} else {
-		ssize_t sz = lbs_tlv_size(events->tlv, sizeof(events->tlv));
-		cmd->size = cpu_to_le16(le16_to_cpu(cmd->size) + sz);
-		cmd->params.subscribe_event.events = events->events;
-		memcpy(cmd->params.subscribe_event.tlv, events->tlv, sz);
-	}
+	ret = lbs_cmd_with_response(priv, CMD_802_11_ENABLE_RSN, &cmd);
+	if (!ret && cmd_action == CMD_ACT_GET)
+		*enable = le16_to_cpu(cmd.enable);
 
-	lbs_deb_leave(LBS_DEB_CMD);
+	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
+	return ret;
 }
 
 static void set_one_wpa_key(struct MrvlIEtype_keyParamSet * pkeyparamset,
@@ -582,45 +559,6 @@ static int lbs_cmd_802_11_snmp_mib(struct lbs_private *priv,
 	return 0;
 }
 
-static int lbs_cmd_802_11_radio_control(struct lbs_private *priv,
-					 struct cmd_ds_command *cmd,
-					 int cmd_action)
-{
-	struct cmd_ds_802_11_radio_control *pradiocontrol = &cmd->params.radio;
-
-	lbs_deb_enter(LBS_DEB_CMD);
-
-	cmd->size =
-	    cpu_to_le16((sizeof(struct cmd_ds_802_11_radio_control)) +
-			     S_DS_GEN);
-	cmd->command = cpu_to_le16(CMD_802_11_RADIO_CONTROL);
-
-	pradiocontrol->action = cpu_to_le16(cmd_action);
-
-	switch (priv->preamble) {
-	case CMD_TYPE_SHORT_PREAMBLE:
-		pradiocontrol->control = cpu_to_le16(SET_SHORT_PREAMBLE);
-		break;
-
-	case CMD_TYPE_LONG_PREAMBLE:
-		pradiocontrol->control = cpu_to_le16(SET_LONG_PREAMBLE);
-		break;
-
-	case CMD_TYPE_AUTO_PREAMBLE:
-	default:
-		pradiocontrol->control = cpu_to_le16(SET_AUTO_PREAMBLE);
-		break;
-	}
-
-	if (priv->radioon)
-		pradiocontrol->control |= cpu_to_le16(TURN_ON_RF);
-	else
-		pradiocontrol->control &= cpu_to_le16(~TURN_ON_RF);
-
-	lbs_deb_leave(LBS_DEB_CMD);
-	return 0;
-}
-
 static int lbs_cmd_802_11_rf_tx_power(struct lbs_private *priv,
 				       struct cmd_ds_command *cmd,
 				       u16 cmd_action, void *pdata_buf)
@@ -724,7 +662,7 @@ int lbs_get_data_rate(struct lbs_private *priv)
 	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 	cmd.action = cpu_to_le16(CMD_ACT_GET_TX_RATE);
 
-	ret = lbs_cmd_with_response(priv, CMD_802_11_DATA_RATE, cmd);
+	ret = lbs_cmd_with_response(priv, CMD_802_11_DATA_RATE, &cmd);
 	if (ret)
 		goto out;
 
@@ -771,7 +709,7 @@ int lbs_set_data_rate(struct lbs_private *priv, u8 rate)
 		lbs_deb_cmd("DATA_RATE: setting auto\n");
 	}
 
-	ret = lbs_cmd_with_response(priv, CMD_802_11_DATA_RATE, cmd);
+	ret = lbs_cmd_with_response(priv, CMD_802_11_DATA_RATE, &cmd);
 	if (ret)
 		goto out;
 
@@ -827,7 +765,7 @@ int lbs_get_channel(struct lbs_private *priv)
 	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
 	cmd.action = cpu_to_le16(CMD_OPT_802_11_RF_CHANNEL_GET);
 
-	ret = lbs_cmd_with_response(priv, CMD_802_11_RF_CHANNEL, cmd);
+	ret = lbs_cmd_with_response(priv, CMD_802_11_RF_CHANNEL, &cmd);
 	if (ret)
 		goto out;
 
@@ -859,7 +797,7 @@ int lbs_set_channel(struct lbs_private *priv, u8 channel)
 	cmd.action = cpu_to_le16(CMD_OPT_802_11_RF_CHANNEL_SET);
 	cmd.channel = cpu_to_le16(channel);
 
-	ret = lbs_cmd_with_response(priv, CMD_802_11_RF_CHANNEL, cmd);
+	ret = lbs_cmd_with_response(priv, CMD_802_11_RF_CHANNEL, &cmd);
 	if (ret)
 		goto out;
 
@@ -1081,33 +1019,36 @@ int lbs_mesh_access(struct lbs_private *priv, uint16_t cmd_action,
 	lbs_deb_enter_args(LBS_DEB_CMD, "action %d", cmd_action);
 
 	cmd->hdr.command = cpu_to_le16(CMD_MESH_ACCESS);
-	cmd->hdr.size = cpu_to_le16(sizeof(struct cmd_ds_mesh_access) + S_DS_GEN);
+	cmd->hdr.size = cpu_to_le16(sizeof(*cmd));
 	cmd->hdr.result = 0;
 
 	cmd->action = cpu_to_le16(cmd_action);
 
-	ret = lbs_cmd_with_response(priv, CMD_MESH_ACCESS, (*cmd));
+	ret = lbs_cmd_with_response(priv, CMD_MESH_ACCESS, cmd);
 
 	lbs_deb_leave(LBS_DEB_CMD);
 	return ret;
 }
 EXPORT_SYMBOL_GPL(lbs_mesh_access);
 
-int lbs_mesh_config(struct lbs_private *priv, int enable)
+int lbs_mesh_config(struct lbs_private *priv, uint16_t enable, uint16_t chan)
 {
 	struct cmd_ds_mesh_config cmd;
 
 	memset(&cmd, 0, sizeof(cmd));
 	cmd.action = cpu_to_le16(enable);
-	cmd.channel = cpu_to_le16(priv->curbssparams.channel);
-	cmd.type = cpu_to_le16(0x100 + 37);
-	
+	cmd.channel = cpu_to_le16(chan);
+	cmd.type = cpu_to_le16(priv->mesh_tlv);
+	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
+
 	if (enable) {
 		cmd.length = cpu_to_le16(priv->mesh_ssid_len);
 		memcpy(cmd.data, priv->mesh_ssid, priv->mesh_ssid_len);
 	}
-
-	return lbs_cmd_with_response(priv, CMD_MESH_CONFIG, cmd);
+	lbs_deb_cmd("mesh config enable %d TLV %x channel %d SSID %s\n",
+		    enable, priv->mesh_tlv, chan,
+		    escape_essid(priv->mesh_ssid, priv->mesh_ssid_len));
+	return lbs_cmd_with_response(priv, CMD_MESH_CONFIG, &cmd);
 }
 
 static int lbs_cmd_bcn_ctrl(struct lbs_private * priv,
@@ -1131,26 +1072,27 @@ static int lbs_cmd_bcn_ctrl(struct lbs_private * priv,
 	return 0;
 }
 
-/*
- * Note: NEVER use lbs_queue_cmd() with addtail==0 other than for
- * the command timer, because it does not account for queued commands.
- */
-void lbs_queue_cmd(struct lbs_private *priv,
-	struct cmd_ctrl_node *cmdnode,
-	u8 addtail)
+static void lbs_queue_cmd(struct lbs_private *priv,
+			  struct cmd_ctrl_node *cmdnode)
 {
 	unsigned long flags;
+	int addtail = 1;
 
 	lbs_deb_enter(LBS_DEB_HOST);
 
-	if (!cmdnode || !cmdnode->cmdbuf) {
-		lbs_deb_host("QUEUE_CMD: cmdnode or cmdbuf is NULL\n");
+	if (!cmdnode) {
+		lbs_deb_host("QUEUE_CMD: cmdnode is NULL\n");
+		goto done;
+	}
+	if (!cmdnode->cmdbuf->size) {
+		lbs_deb_host("DNLD_CMD: cmd size is zero\n");
 		goto done;
 	}
+	cmdnode->result = 0;
 
 	/* Exit_PS command needs to be queued in the header always. */
 	if (le16_to_cpu(cmdnode->cmdbuf->command) == CMD_802_11_PS_MODE) {
-		struct cmd_ds_802_11_ps_mode *psm = (void *) cmdnode->cmdbuf;
+		struct cmd_ds_802_11_ps_mode *psm = (void *) &cmdnode->cmdbuf[1];
 
 		if (psm->action == cpu_to_le16(CMD_SUBCMD_EXIT_PS)) {
 			if (priv->psstate != PS_STATE_FULL_POWER)
@@ -1168,45 +1110,27 @@ void lbs_queue_cmd(struct lbs_private *priv,
 	spin_unlock_irqrestore(&priv->driver_lock, flags);
 
 	lbs_deb_host("QUEUE_CMD: inserted command 0x%04x into cmdpendingq\n",
-	       le16_to_cpu(cmdnode->cmdbuf->command));
+		     le16_to_cpu(cmdnode->cmdbuf->command));
 
 done:
 	lbs_deb_leave(LBS_DEB_HOST);
 }
 
-/*
- * TODO: Fix the issue when DownloadcommandToStation is being called the
- * second time when the command times out. All the cmdptr->xxx are in little
- * endian and therefore all the comparissions will fail.
- * For now - we are not performing the endian conversion the second time - but
- * for PS and DEEP_SLEEP we need to worry
- */
-static int DownloadcommandToStation(struct lbs_private *priv,
-				    struct cmd_ctrl_node *cmdnode)
+static void lbs_submit_command(struct lbs_private *priv,
+			       struct cmd_ctrl_node *cmdnode)
 {
 	unsigned long flags;
 	struct cmd_header *cmd;
-	int ret = -1;
-	u16 cmdsize;
-	u16 command;
+	uint16_t cmdsize;
+	uint16_t command;
+	int timeo = 5 * HZ;
+	int ret;
 
 	lbs_deb_enter(LBS_DEB_HOST);
 
-	if (!priv || !cmdnode) {
-		lbs_deb_host("DNLD_CMD: priv or cmdmode is NULL\n");
-		goto done;
-	}
-
 	cmd = cmdnode->cmdbuf;
 
 	spin_lock_irqsave(&priv->driver_lock, flags);
-	if (!cmd || !cmd->size) {
-		lbs_deb_host("DNLD_CMD: cmdptr is NULL or zero\n");
-		__lbs_cleanup_and_insert_cmd(priv, cmdnode);
-		spin_unlock_irqrestore(&priv->driver_lock, flags);
-		goto done;
-	}
-
 	priv->cur_cmd = cmdnode;
 	priv->cur_cmd_retcode = 0;
 	spin_unlock_irqrestore(&priv->driver_lock, flags);
@@ -1214,38 +1138,30 @@ static int DownloadcommandToStation(struct lbs_private *priv,
 	cmdsize = le16_to_cpu(cmd->size);
 	command = le16_to_cpu(cmd->command);
 
-	lbs_deb_host("DNLD_CMD: command 0x%04x, size %d, jiffies %lu\n",
-		    command, cmdsize, jiffies);
-	lbs_deb_hex(LBS_DEB_HOST, "DNLD_CMD", (void *) cmdnode->cmdbuf, cmdsize);
+	/* These commands take longer */
+	if (command == CMD_802_11_SCAN || command == CMD_802_11_ASSOCIATE ||
+	    command == CMD_802_11_AUTHENTICATE)
+		timeo = 10 * HZ;
 
-	cmdnode->cmdwaitqwoken = 0;
+	lbs_deb_host("DNLD_CMD: command 0x%04x, seq %d, size %d, jiffies %lu\n",
+		     command, le16_to_cpu(cmd->seqnum), cmdsize, jiffies);
+	lbs_deb_hex(LBS_DEB_HOST, "DNLD_CMD", (void *) cmdnode->cmdbuf, cmdsize);
 
 	ret = priv->hw_host_to_card(priv, MVMS_CMD, (u8 *) cmd, cmdsize);
 
-	if (ret != 0) {
-		lbs_deb_host("DNLD_CMD: hw_host_to_card failed\n");
-		spin_lock_irqsave(&priv->driver_lock, flags);
-		priv->cur_cmd_retcode = ret;
-		__lbs_cleanup_and_insert_cmd(priv, priv->cur_cmd);
-		priv->cur_cmd = NULL;
-		spin_unlock_irqrestore(&priv->driver_lock, flags);
-		goto done;
-	}
-
-	lbs_deb_cmd("DNLD_CMD: sent command 0x%04x, jiffies %lu\n", command, jiffies);
+	if (ret) {
+		lbs_pr_info("DNLD_CMD: hw_host_to_card failed: %d\n", ret);
+		/* Let the timer kick in and retry, and potentially reset
+		   the whole thing if the condition persists */
+		timeo = HZ;
+	} else
+		lbs_deb_cmd("DNLD_CMD: sent command 0x%04x, jiffies %lu\n",
+			    command, jiffies);
 
 	/* Setup the timer after transmit command */
-	if (command == CMD_802_11_SCAN || command == CMD_802_11_AUTHENTICATE
-	    || command == CMD_802_11_ASSOCIATE)
-		mod_timer(&priv->command_timer, jiffies + (10*HZ));
-	else
-		mod_timer(&priv->command_timer, jiffies + (5*HZ));
-
-	ret = 0;
+	mod_timer(&priv->command_timer, jiffies + timeo);
 
-done:
-	lbs_deb_leave_args(LBS_DEB_HOST, "ret %d", ret);
-	return ret;
+	lbs_deb_leave(LBS_DEB_HOST);
 }
 
 static int lbs_cmd_mac_control(struct lbs_private *priv,
@@ -1270,15 +1186,22 @@ static int lbs_cmd_mac_control(struct lbs_private *priv,
  *  This function inserts command node to cmdfreeq
  *  after cleans it. Requires priv->driver_lock held.
  */
-void __lbs_cleanup_and_insert_cmd(struct lbs_private *priv,
-	struct cmd_ctrl_node *ptempcmd)
+static void __lbs_cleanup_and_insert_cmd(struct lbs_private *priv,
+					 struct cmd_ctrl_node *cmdnode)
 {
+	lbs_deb_enter(LBS_DEB_HOST);
 
-	if (!ptempcmd)
-		return;
+	if (!cmdnode)
+		goto out;
 
-	cleanup_cmdnode(ptempcmd);
-	list_add_tail(&ptempcmd->list, &priv->cmdfreeq);
+	cmdnode->callback = NULL;
+	cmdnode->callback_arg = 0;
+
+	memset(cmdnode->cmdbuf, 0, LBS_CMD_BUFFER_SIZE);
+
+	list_add_tail(&cmdnode->list, &priv->cmdfreeq);
+ out:
+	lbs_deb_leave(LBS_DEB_HOST);
 }
 
 static void lbs_cleanup_and_insert_cmd(struct lbs_private *priv,
@@ -1291,19 +1214,55 @@ static void lbs_cleanup_and_insert_cmd(struct lbs_private *priv,
 	spin_unlock_irqrestore(&priv->driver_lock, flags);
 }
 
+void lbs_complete_command(struct lbs_private *priv, struct cmd_ctrl_node *cmd,
+			  int result)
+{
+	if (cmd == priv->cur_cmd)
+		priv->cur_cmd_retcode = result;
+
+	cmd->result = result;
+	cmd->cmdwaitqwoken = 1;
+	wake_up_interruptible(&cmd->cmdwait_q);
+
+	if (!cmd->callback)
+		__lbs_cleanup_and_insert_cmd(priv, cmd);
+	priv->cur_cmd = NULL;
+}
+
 int lbs_set_radio_control(struct lbs_private *priv)
 {
 	int ret = 0;
+	struct cmd_ds_802_11_radio_control cmd;
 
 	lbs_deb_enter(LBS_DEB_CMD);
 
-	ret = lbs_prepare_and_send_command(priv,
-				    CMD_802_11_RADIO_CONTROL,
-				    CMD_ACT_SET,
-				    CMD_OPTION_WAITFORRSP, 0, NULL);
+	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
+	cmd.action = cpu_to_le16(CMD_ACT_SET);
 
-	lbs_deb_cmd("RADIO_SET: radio %d, preamble %d\n",
-	       priv->radioon, priv->preamble);
+	switch (priv->preamble) {
+	case CMD_TYPE_SHORT_PREAMBLE:
+		cmd.control = cpu_to_le16(SET_SHORT_PREAMBLE);
+		break;
+
+	case CMD_TYPE_LONG_PREAMBLE:
+		cmd.control = cpu_to_le16(SET_LONG_PREAMBLE);
+		break;
+
+	case CMD_TYPE_AUTO_PREAMBLE:
+	default:
+		cmd.control = cpu_to_le16(SET_AUTO_PREAMBLE);
+		break;
+	}
+
+	if (priv->radioon)
+		cmd.control |= cpu_to_le16(TURN_ON_RF);
+	else
+		cmd.control &= cpu_to_le16(~TURN_ON_RF);
+
+	lbs_deb_cmd("RADIO_SET: radio %d, preamble %d\n", priv->radioon,
+		    priv->preamble);
+
+	ret = lbs_cmd_with_response(priv, CMD_802_11_RADIO_CONTROL, &cmd);
 
 	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 	return ret;
@@ -1369,19 +1328,12 @@ int lbs_prepare_and_send_command(struct lbs_private *priv,
 		goto done;
 	}
 
-	lbs_set_cmd_ctrl_node(priv, cmdnode, wait_option, pdata_buf);
+	lbs_set_cmd_ctrl_node(priv, cmdnode, pdata_buf);
 
 	cmdptr = (struct cmd_ds_command *)cmdnode->cmdbuf;
 
 	lbs_deb_host("PREP_CMD: command 0x%04x\n", cmd_no);
 
-	if (!cmdptr) {
-		lbs_deb_host("PREP_CMD: cmdptr is NULL\n");
-		lbs_cleanup_and_insert_cmd(priv, cmdnode);
-		ret = -1;
-		goto done;
-	}
-
 	/* Set sequence number, command and INT option */
 	priv->seqnum++;
 	cmdptr->seqnum = cpu_to_le16(priv->seqnum);
@@ -1411,10 +1363,6 @@ int lbs_prepare_and_send_command(struct lbs_private *priv,
 		ret = lbs_cmd_80211_deauthenticate(priv, cmdptr);
 		break;
 
-	case CMD_802_11_SET_WEP:
-		ret = lbs_cmd_802_11_set_wep(priv, cmdptr, cmd_action, pdata_buf);
-		break;
-
 	case CMD_802_11_AD_HOC_START:
 		ret = lbs_cmd_80211_ad_hoc_start(priv, cmdptr, pdata_buf);
 		break;
@@ -1453,10 +1401,6 @@ int lbs_prepare_and_send_command(struct lbs_private *priv,
 						  cmd_action, pdata_buf);
 		break;
 
-	case CMD_802_11_RADIO_CONTROL:
-		ret = lbs_cmd_802_11_radio_control(priv, cmdptr, cmd_action);
-		break;
-
 	case CMD_802_11_RATE_ADAPT_RATESET:
 		ret = lbs_cmd_802_11_rate_adapt_rateset(priv,
 							 cmdptr, cmd_action);
@@ -1483,11 +1427,6 @@ int lbs_prepare_and_send_command(struct lbs_private *priv,
 		ret = lbs_cmd_80211_ad_hoc_stop(priv, cmdptr);
 		break;
 
-	case CMD_802_11_ENABLE_RSN:
-		ret = lbs_cmd_802_11_enable_rsn(priv, cmdptr, cmd_action,
-				pdata_buf);
-		break;
-
 	case CMD_802_11_KEY_MATERIAL:
 		ret = lbs_cmd_802_11_key_material(priv, cmdptr, cmd_action,
 				cmd_oid, pdata_buf);
@@ -1525,15 +1464,6 @@ int lbs_prepare_and_send_command(struct lbs_private *priv,
 						   cmd_no, cmd_action);
 		break;
 
-	case CMD_802_11_SLEEP_PARAMS:
-		ret = lbs_cmd_802_11_sleep_params(priv, cmdptr, cmd_action);
-		break;
-	case CMD_802_11_INACTIVITY_TIMEOUT:
-		ret = lbs_cmd_802_11_inactivity_timeout(priv, cmdptr,
-							 cmd_action, pdata_buf);
-		lbs_set_cmd_ctrl_node(priv, cmdnode, 0, pdata_buf);
-		break;
-
 	case CMD_802_11_TPC_CFG:
 		cmdptr->command = cpu_to_le16(CMD_802_11_TPC_CFG);
 		cmdptr->size =
@@ -1568,10 +1498,7 @@ int lbs_prepare_and_send_command(struct lbs_private *priv,
 			ret = 0;
 			break;
 		}
-	case CMD_802_11_SUBSCRIBE_EVENT:
-		lbs_cmd_802_11_subscribe_event(priv, cmdptr,
-			cmd_action, pdata_buf);
-		break;
+
 	case CMD_802_11_PWR_CFG:
 		cmdptr->command = cpu_to_le16(CMD_802_11_PWR_CFG);
 		cmdptr->size =
@@ -1615,7 +1542,7 @@ int lbs_prepare_and_send_command(struct lbs_private *priv,
 
 	cmdnode->cmdwaitqwoken = 0;
 
-	lbs_queue_cmd(priv, cmdnode, 1);
+	lbs_queue_cmd(priv, cmdnode);
 	wake_up_interruptible(&priv->waitq);
 
 	if (wait_option & CMD_OPTION_WAITFORRSP) {
@@ -1756,9 +1683,6 @@ static struct cmd_ctrl_node *lbs_get_cmd_ctrl_node(struct lbs_private *priv)
 
 	spin_unlock_irqrestore(&priv->driver_lock, flags);
 
-	if (tempnode)
-		cleanup_cmdnode(tempnode);
-
 	lbs_deb_leave(LBS_DEB_HOST);
 	return tempnode;
 }
@@ -1769,47 +1693,26 @@ static struct cmd_ctrl_node *lbs_get_cmd_ctrl_node(struct lbs_private *priv)
  *  @param ptempnode	A pointer to cmdCtrlNode structure
  *  @return 		n/a
  */
-static void cleanup_cmdnode(struct cmd_ctrl_node *cmdnode)
-{
-	lbs_deb_enter(LBS_DEB_HOST);
-
-	if (!cmdnode)
-		return;
-	cmdnode->cmdwaitqwoken = 1;
-	wake_up_interruptible(&cmdnode->cmdwait_q);
-	cmdnode->wait_option = 0;
-	cmdnode->pdata_buf = NULL;
-	cmdnode->callback = NULL;
-	cmdnode->callback_arg = 0;
-
-	if (cmdnode->cmdbuf != NULL)
-		memset(cmdnode->cmdbuf, 0, LBS_CMD_BUFFER_SIZE);
-
-	lbs_deb_leave(LBS_DEB_HOST);
-}
 
 /**
  *  @brief This function initializes the command node.
  *
  *  @param priv		A pointer to struct lbs_private structure
  *  @param ptempnode	A pointer to cmd_ctrl_node structure
- *  @param wait_option	wait option: wait response or not
  *  @param pdata_buf	A pointer to informaion buffer
  *  @return 		0 or -1
  */
 static void lbs_set_cmd_ctrl_node(struct lbs_private *priv,
 				  struct cmd_ctrl_node *ptempnode,
-				  u16 wait_option, void *pdata_buf)
+				  void *pdata_buf)
 {
 	lbs_deb_enter(LBS_DEB_HOST);
 
 	if (!ptempnode)
 		return;
 
-	ptempnode->wait_option = wait_option;
-	ptempnode->pdata_buf = pdata_buf;
 	ptempnode->callback = NULL;
-	ptempnode->callback_arg = 0;
+	ptempnode->callback_arg = (unsigned long)pdata_buf;
 
 	lbs_deb_leave(LBS_DEB_HOST);
 }
@@ -1897,7 +1800,7 @@ int lbs_execute_next_command(struct lbs_private *priv)
 				 * PS command. Ignore it if it is not Exit_PS.
 				 * otherwise send it down immediately.
 				 */
-				struct cmd_ds_802_11_ps_mode *psm = (void *)cmd;
+				struct cmd_ds_802_11_ps_mode *psm = (void *)&cmd[1];
 
 				lbs_deb_host(
 				       "EXEC_NEXT_CMD: PS cmd, action 0x%02x\n",
@@ -1907,7 +1810,9 @@ int lbs_execute_next_command(struct lbs_private *priv)
 					lbs_deb_host(
 					       "EXEC_NEXT_CMD: ignore ENTER_PS cmd\n");
 					list_del(&cmdnode->list);
-					lbs_cleanup_and_insert_cmd(priv, cmdnode);
+					spin_lock_irqsave(&priv->driver_lock, flags);
+					lbs_complete_command(priv, cmdnode, 0);
+					spin_unlock_irqrestore(&priv->driver_lock, flags);
 
 					ret = 0;
 					goto done;
@@ -1918,7 +1823,9 @@ int lbs_execute_next_command(struct lbs_private *priv)
 					lbs_deb_host(
 					       "EXEC_NEXT_CMD: ignore EXIT_PS cmd in sleep\n");
 					list_del(&cmdnode->list);
-					lbs_cleanup_and_insert_cmd(priv, cmdnode);
+					spin_lock_irqsave(&priv->driver_lock, flags);
+					lbs_complete_command(priv, cmdnode, 0);
+					spin_unlock_irqrestore(&priv->driver_lock, flags);
 					priv->needtowakeup = 1;
 
 					ret = 0;
@@ -1932,7 +1839,7 @@ int lbs_execute_next_command(struct lbs_private *priv)
 		list_del(&cmdnode->list);
 		lbs_deb_host("EXEC_NEXT_CMD: sending command 0x%04x\n",
 			    le16_to_cpu(cmd->command));
-		DownloadcommandToStation(priv, cmdnode);
+		lbs_submit_command(priv, cmdnode);
 	} else {
 		/*
 		 * check if in power save mode, if yes, put the device back
@@ -2004,7 +1911,6 @@ static int sendconfirmsleep(struct lbs_private *priv, u8 *cmdptr, u16 size)
 	lbs_deb_hex(LBS_DEB_HOST, "sleep confirm command", cmdptr, size);
 
 	ret = priv->hw_host_to_card(priv, MVMS_CMD, cmdptr, size);
-	priv->dnld_sent = DNLD_RES_RECEIVED;
 
 	spin_lock_irqsave(&priv->driver_lock, flags);
 	if (priv->intcounter || priv->currenttxskb)
@@ -2086,17 +1992,17 @@ void lbs_ps_confirm_sleep(struct lbs_private *priv, u16 psmode)
 
 	if (priv->dnld_sent) {
 		allowed = 0;
-		lbs_deb_host("dnld_sent was set");
+		lbs_deb_host("dnld_sent was set\n");
 	}
 
 	spin_lock_irqsave(&priv->driver_lock, flags);
 	if (priv->cur_cmd) {
 		allowed = 0;
-		lbs_deb_host("cur_cmd was set");
+		lbs_deb_host("cur_cmd was set\n");
 	}
 	if (priv->intcounter > 0) {
 		allowed = 0;
-		lbs_deb_host("intcounter %d", priv->intcounter);
+		lbs_deb_host("intcounter %d\n", priv->intcounter);
 	}
 	spin_unlock_irqrestore(&priv->driver_lock, flags);
 
@@ -2139,43 +2045,20 @@ int lbs_cmd_copyback(struct lbs_private *priv, unsigned long extra,
 	lbs_deb_leave(LBS_DEB_CMD);
 	return 0;
 }
+EXPORT_SYMBOL_GPL(lbs_cmd_copyback);
 
-/**
- *  @brief Simple way to call firmware functions
- *
- *  @param priv    	A pointer to struct lbs_private structure
- *  @param psmode  	one of the many CMD_802_11_xxxx
- *  @param cmd          pointer to the parameters structure for above command
- *                      (this should not include the command, size, sequence
- *                      and result fields from struct cmd_ds_gen)
- *  @param cmd_size     size structure pointed to by cmd
- *  @param rsp          pointer to an area where the result should be placed
- *  @param rsp_size     pointer to the size of the rsp area. If the firmware
- *                      returns fewer bytes, then this *rsp_size will be
- *                      changed to the actual size.
- *  @return 	   	-1 in case of a higher level error, otherwise
- *                      the result code from the firmware
- */
-int __lbs_cmd(struct lbs_private *priv, uint16_t command,
-	      struct cmd_header *in_cmd, int in_cmd_size,
-	      int (*callback)(struct lbs_private *, unsigned long, struct cmd_header *),
-	      unsigned long callback_arg)
+struct cmd_ctrl_node *__lbs_cmd_async(struct lbs_private *priv, uint16_t command,
+				      struct cmd_header *in_cmd, int in_cmd_size,
+				      int (*callback)(struct lbs_private *, unsigned long, struct cmd_header *),
+				      unsigned long callback_arg)
 {
 	struct cmd_ctrl_node *cmdnode;
-	unsigned long flags;
-	int ret = 0;
 
 	lbs_deb_enter(LBS_DEB_HOST);
 
-	if (!priv) {
-		lbs_deb_host("PREP_CMD: priv is NULL\n");
-		ret = -1;
-		goto done;
-	}
-
 	if (priv->surpriseremoved) {
 		lbs_deb_host("PREP_CMD: card removed\n");
-		ret = -1;
+		cmdnode = ERR_PTR(-ENOENT);
 		goto done;
 	}
 
@@ -2185,11 +2068,10 @@ int __lbs_cmd(struct lbs_private *priv, uint16_t command,
 
 		/* Wake up main thread to execute next command */
 		wake_up_interruptible(&priv->waitq);
-		ret = -1;
+		cmdnode = ERR_PTR(-ENOBUFS);
 		goto done;
 	}
 
-	cmdnode->wait_option = CMD_OPTION_WAITFORRSP;
 	cmdnode->callback = callback;
 	cmdnode->callback_arg = callback_arg;
 
@@ -2209,19 +2091,42 @@ int __lbs_cmd(struct lbs_private *priv, uint16_t command,
 	 * because the caller of lbs_cmd() sets up all of *cmd for us. */
 
 	cmdnode->cmdwaitqwoken = 0;
-	lbs_queue_cmd(priv, cmdnode, 1);
+	lbs_queue_cmd(priv, cmdnode);
 	wake_up_interruptible(&priv->waitq);
 
+ done:
+	lbs_deb_leave_args(LBS_DEB_HOST, "ret %p", cmdnode);
+	return cmdnode;
+}
+
+int __lbs_cmd(struct lbs_private *priv, uint16_t command,
+	      struct cmd_header *in_cmd, int in_cmd_size,
+	      int (*callback)(struct lbs_private *, unsigned long, struct cmd_header *),
+	      unsigned long callback_arg)
+{
+	struct cmd_ctrl_node *cmdnode;
+	unsigned long flags;
+	int ret = 0;
+
+	lbs_deb_enter(LBS_DEB_HOST);
+
+	cmdnode = __lbs_cmd_async(priv, command, in_cmd, in_cmd_size,
+				  callback, callback_arg);
+	if (IS_ERR(cmdnode)) {
+		ret = PTR_ERR(cmdnode);
+		goto done;
+	}
+
 	might_sleep();
 	wait_event_interruptible(cmdnode->cmdwait_q, cmdnode->cmdwaitqwoken);
 
 	spin_lock_irqsave(&priv->driver_lock, flags);
-	if (priv->cur_cmd_retcode) {
-		lbs_deb_host("PREP_CMD: command failed with return code %d\n",
-		       priv->cur_cmd_retcode);
-		priv->cur_cmd_retcode = 0;
-		ret = -1;
-	}
+	ret = cmdnode->result;
+	if (ret)
+		lbs_pr_info("PREP_CMD: command 0x%04x failed: %d\n",
+			    command, ret);
+
+	__lbs_cleanup_and_insert_cmd(priv, cmdnode);
 	spin_unlock_irqrestore(&priv->driver_lock, flags);
 
 done:
diff --git a/package/libertas/src/cmd.h b/package/libertas/src/cmd.h
index 80714b5128..b9ab85cc79 100644
--- a/package/libertas/src/cmd.h
+++ b/package/libertas/src/cmd.h
@@ -6,16 +6,26 @@
 #include "hostcmd.h"
 #include "dev.h"
 
-#define lbs_cmd(priv, cmdnr, cmd, callback, callback_arg) \
-	__lbs_cmd(priv, cmdnr, &(cmd).hdr, sizeof(cmd),	  \
-			callback, callback_arg)
-
-#define lbs_cmd_with_response(priv, cmdnr, cmd) \
-	__lbs_cmd(priv, cmdnr, &(cmd).hdr, sizeof(cmd), \
-		  lbs_cmd_copyback, (unsigned long) &cmd)
- 
+/* lbs_cmd() infers the size of the buffer to copy data back into, from
+   the size of the target of the pointer. Since the command to be sent 
+   may often be smaller, that size is set in cmd->size by the caller.*/
+#define lbs_cmd(priv, cmdnr, cmd, cb, cb_arg)	({		\
+	uint16_t __sz = le16_to_cpu((cmd)->hdr.size);		\
+	(cmd)->hdr.size = cpu_to_le16(sizeof(*(cmd)));		\
+	__lbs_cmd(priv, cmdnr, &(cmd)->hdr, __sz, cb, cb_arg);	\
+})
+
+#define lbs_cmd_with_response(priv, cmdnr, cmd)	\
+	lbs_cmd(priv, cmdnr, cmd, lbs_cmd_copyback, (unsigned long) (cmd))
+
+/* __lbs_cmd() will free the cmdnode and return success/failure.
+   __lbs_cmd_async() requires that the callback free the cmdnode */
+struct cmd_ctrl_node *__lbs_cmd_async(struct lbs_private *priv, uint16_t command,
+				      struct cmd_header *in_cmd, int in_cmd_size,
+				      int (*callback)(struct lbs_private *, unsigned long, struct cmd_header *),
+				      unsigned long callback_arg);
 int __lbs_cmd(struct lbs_private *priv, uint16_t command,
-	      struct cmd_header *in_cmd, int in_cmd_size, 
+	      struct cmd_header *in_cmd, int in_cmd_size,
 	      int (*callback)(struct lbs_private *, unsigned long, struct cmd_header *),
 	      unsigned long callback_arg);
 
@@ -33,6 +43,19 @@ int lbs_set_data_rate(struct lbs_private *priv, u8 rate);
 int lbs_get_channel(struct lbs_private *priv);
 int lbs_set_channel(struct lbs_private *priv, u8 channel);
 
-int lbs_mesh_config(struct lbs_private *priv, int enable);
+int lbs_mesh_config(struct lbs_private *priv, uint16_t enable, uint16_t chan);
+
+int lbs_host_sleep_cfg(struct lbs_private *priv, uint32_t criteria);
+int lbs_suspend(struct lbs_private *priv);
+int lbs_resume(struct lbs_private *priv);
+
+int lbs_cmd_802_11_inactivity_timeout(struct lbs_private *priv,
+				      uint16_t cmd_action, uint16_t *timeout);
+int lbs_cmd_802_11_sleep_params(struct lbs_private *priv, uint16_t cmd_action,
+				struct sleep_params *sp);
+int lbs_cmd_802_11_set_wep(struct lbs_private *priv, uint16_t cmd_action,
+			   struct assoc_request *assoc);
+int lbs_cmd_802_11_enable_rsn(struct lbs_private *priv, uint16_t cmd_action,
+			      uint16_t *enable);
 
 #endif /* _LBS_CMD_H */
diff --git a/package/libertas/src/cmdresp.c b/package/libertas/src/cmdresp.c
index bf9941ecc2..159216a919 100644
--- a/package/libertas/src/cmdresp.c
+++ b/package/libertas/src/cmdresp.c
@@ -43,14 +43,15 @@ void lbs_mac_event_disconnected(struct lbs_private *priv)
 	msleep_interruptible(1000);
 	wireless_send_event(priv->dev, SIOCGIWAP, &wrqu, NULL);
 
-	/* Free Tx and Rx packets */
-	kfree_skb(priv->currenttxskb);
-	priv->currenttxskb = NULL;
-
 	/* report disconnect to upper layer */
 	netif_stop_queue(priv->dev);
 	netif_carrier_off(priv->dev);
 
+	/* Free Tx and Rx packets */
+	kfree_skb(priv->currenttxskb);
+	priv->currenttxskb = NULL;
+	priv->tx_pending_len = 0;
+
 	/* reset SNR/NF/RSSI values */
 	memset(priv->SNR, 0x00, sizeof(priv->SNR));
 	memset(priv->NF, 0x00, sizeof(priv->NF));
@@ -145,29 +146,6 @@ static int lbs_ret_reg_access(struct lbs_private *priv,
 	return ret;
 }
 
-static int lbs_ret_802_11_sleep_params(struct lbs_private *priv,
-					struct cmd_ds_command *resp)
-{
-	struct cmd_ds_802_11_sleep_params *sp = &resp->params.sleep_params;
-
-	lbs_deb_enter(LBS_DEB_CMD);
-
-	lbs_deb_cmd("error 0x%x, offset 0x%x, stabletime 0x%x, calcontrol 0x%x "
-		    "extsleepclk 0x%x\n", le16_to_cpu(sp->error),
-		    le16_to_cpu(sp->offset), le16_to_cpu(sp->stabletime),
-		    sp->calcontrol, sp->externalsleepclk);
-
-	priv->sp.sp_error = le16_to_cpu(sp->error);
-	priv->sp.sp_offset = le16_to_cpu(sp->offset);
-	priv->sp.sp_stabletime = le16_to_cpu(sp->stabletime);
-	priv->sp.sp_calcontrol = sp->calcontrol;
-	priv->sp.sp_extsleepclk = sp->externalsleepclk;
-	priv->sp.sp_reserved = le16_to_cpu(sp->reserved);
-
-	lbs_deb_enter(LBS_DEB_CMD);
-	return 0;
-}
-
 static int lbs_ret_802_11_stat(struct lbs_private *priv,
 				struct cmd_ds_command *resp)
 {
@@ -394,23 +372,6 @@ static int lbs_ret_get_log(struct lbs_private *priv,
 	return 0;
 }
 
-static int lbs_ret_802_11_enable_rsn(struct lbs_private *priv,
-                                          struct cmd_ds_command *resp)
-{
-	struct cmd_ds_802_11_enable_rsn *enable_rsn = &resp->params.enbrsn;
-	u32 * pdata_buf = priv->cur_cmd->pdata_buf;
-
-	lbs_deb_enter(LBS_DEB_CMD);
-
-	if (enable_rsn->action == cpu_to_le16(CMD_ACT_GET)) {
-		if (pdata_buf)
-			*pdata_buf = (u32) le16_to_cpu(enable_rsn->enable);
-	}
-
-	lbs_deb_leave(LBS_DEB_CMD);
-	return 0;
-}
-
 static int lbs_ret_802_11_bcn_ctrl(struct lbs_private * priv,
 					struct cmd_ds_command *resp)
 {
@@ -428,25 +389,6 @@ static int lbs_ret_802_11_bcn_ctrl(struct lbs_private * priv,
 	return 0;
 }
 
-static int lbs_ret_802_11_subscribe_event(struct lbs_private *priv,
-	struct cmd_ds_command *resp)
-{
-	struct cmd_ds_802_11_subscribe_event *cmd_event =
-		&resp->params.subscribe_event;
-	struct cmd_ds_802_11_subscribe_event *dst_event =
-		priv->cur_cmd->pdata_buf;
-
-	lbs_deb_enter(LBS_DEB_CMD);
-
-	if (dst_event->action == cpu_to_le16(CMD_ACT_GET)) {
-		dst_event->events = cmd_event->events;
-		memcpy(dst_event->tlv, cmd_event->tlv, sizeof(dst_event->tlv));
-	}
-
-	lbs_deb_leave(LBS_DEB_CMD);
-	return 0;
-}
-
 static inline int handle_cmd_response(struct lbs_private *priv,
 				      unsigned long dummy,
 				      struct cmd_header *cmd_response)
@@ -504,7 +446,7 @@ static inline int handle_cmd_response(struct lbs_private *priv,
 	case CMD_RET(CMD_802_11_SET_AFC):
 	case CMD_RET(CMD_802_11_GET_AFC):
 		spin_lock_irqsave(&priv->driver_lock, flags);
-		memmove(priv->cur_cmd->pdata_buf, &resp->params.afc,
+		memmove((void *)priv->cur_cmd->callback_arg, &resp->params.afc,
 			sizeof(struct cmd_ds_802_11_afc));
 		spin_unlock_irqrestore(&priv->driver_lock, flags);
 
@@ -512,17 +454,11 @@ static inline int handle_cmd_response(struct lbs_private *priv,
 
 	case CMD_RET(CMD_MAC_MULTICAST_ADR):
 	case CMD_RET(CMD_MAC_CONTROL):
-	case CMD_RET(CMD_802_11_SET_WEP):
 	case CMD_RET(CMD_802_11_RESET):
 	case CMD_RET(CMD_802_11_AUTHENTICATE):
-	case CMD_RET(CMD_802_11_RADIO_CONTROL):
 	case CMD_RET(CMD_802_11_BEACON_STOP):
 		break;
 
-	case CMD_RET(CMD_802_11_ENABLE_RSN):
-		ret = lbs_ret_802_11_enable_rsn(priv, resp);
-		break;
-
 	case CMD_RET(CMD_802_11_RATE_ADAPT_RATESET):
 		ret = lbs_ret_802_11_rate_adapt_rateset(priv, resp);
 		break;
@@ -551,35 +487,22 @@ static inline int handle_cmd_response(struct lbs_private *priv,
 		ret = lbs_ret_802_11d_domain_info(priv, resp);
 		break;
 
-	case CMD_RET(CMD_802_11_SLEEP_PARAMS):
-		ret = lbs_ret_802_11_sleep_params(priv, resp);
-		break;
-	case CMD_RET(CMD_802_11_INACTIVITY_TIMEOUT):
-		spin_lock_irqsave(&priv->driver_lock, flags);
-		*((u16 *) priv->cur_cmd->pdata_buf) =
-		    le16_to_cpu(resp->params.inactivity_timeout.timeout);
-		spin_unlock_irqrestore(&priv->driver_lock, flags);
-		break;
-
 	case CMD_RET(CMD_802_11_TPC_CFG):
 		spin_lock_irqsave(&priv->driver_lock, flags);
-		memmove(priv->cur_cmd->pdata_buf, &resp->params.tpccfg,
+		memmove((void *)priv->cur_cmd->callback_arg, &resp->params.tpccfg,
 			sizeof(struct cmd_ds_802_11_tpc_cfg));
 		spin_unlock_irqrestore(&priv->driver_lock, flags);
 		break;
 	case CMD_RET(CMD_802_11_LED_GPIO_CTRL):
 		spin_lock_irqsave(&priv->driver_lock, flags);
-		memmove(priv->cur_cmd->pdata_buf, &resp->params.ledgpio,
+		memmove((void *)priv->cur_cmd->callback_arg, &resp->params.ledgpio,
 			sizeof(struct cmd_ds_802_11_led_ctrl));
 		spin_unlock_irqrestore(&priv->driver_lock, flags);
 		break;
-	case CMD_RET(CMD_802_11_SUBSCRIBE_EVENT):
-		ret = lbs_ret_802_11_subscribe_event(priv, resp);
-		break;
 
 	case CMD_RET(CMD_802_11_PWR_CFG):
 		spin_lock_irqsave(&priv->driver_lock, flags);
-		memmove(priv->cur_cmd->pdata_buf, &resp->params.pwrcfg,
+		memmove((void *)priv->cur_cmd->callback_arg, &resp->params.pwrcfg,
 			sizeof(struct cmd_ds_802_11_pwr_cfg));
 		spin_unlock_irqrestore(&priv->driver_lock, flags);
 
@@ -587,21 +510,21 @@ static inline int handle_cmd_response(struct lbs_private *priv,
 
 	case CMD_RET(CMD_GET_TSF):
 		spin_lock_irqsave(&priv->driver_lock, flags);
-		memcpy(priv->cur_cmd->pdata_buf,
+		memcpy((void *)priv->cur_cmd->callback_arg,
 		       &resp->params.gettsf.tsfvalue, sizeof(u64));
 		spin_unlock_irqrestore(&priv->driver_lock, flags);
 		break;
 	case CMD_RET(CMD_BT_ACCESS):
 		spin_lock_irqsave(&priv->driver_lock, flags);
-		if (priv->cur_cmd->pdata_buf)
-			memcpy(priv->cur_cmd->pdata_buf,
+		if (priv->cur_cmd->callback_arg)
+			memcpy((void *)priv->cur_cmd->callback_arg,
 			       &resp->params.bt.addr1, 2 * ETH_ALEN);
 		spin_unlock_irqrestore(&priv->driver_lock, flags);
 		break;
 	case CMD_RET(CMD_FWT_ACCESS):
 		spin_lock_irqsave(&priv->driver_lock, flags);
-		if (priv->cur_cmd->pdata_buf)
-			memcpy(priv->cur_cmd->pdata_buf, &resp->params.fwt,
+		if (priv->cur_cmd->callback_arg)
+			memcpy((void *)priv->cur_cmd->callback_arg, &resp->params.fwt,
 			       sizeof(resp->params.fwt));
 		spin_unlock_irqrestore(&priv->driver_lock, flags);
 		break;
@@ -611,7 +534,7 @@ static inline int handle_cmd_response(struct lbs_private *priv,
 
 	default:
 		lbs_deb_host("CMD_RESP: unknown cmd response 0x%04x\n",
-			    resp->command);
+			     le16_to_cpu(resp->command));
 		break;
 	}
 	lbs_deb_leave(LBS_DEB_HOST);
@@ -620,17 +543,14 @@ static inline int handle_cmd_response(struct lbs_private *priv,
 
 int lbs_process_rx_command(struct lbs_private *priv)
 {
-	u16 respcmd;
+	uint16_t respcmd, curcmd;
 	struct cmd_header *resp;
 	int ret = 0;
-	ulong flags;
-	u16 result;
+	unsigned long flags;
+	uint16_t result;
 
 	lbs_deb_enter(LBS_DEB_HOST);
 
-	/* Now we got response from FW, cancel the command timer */
-	del_timer(&priv->command_timer);
-
 	mutex_lock(&priv->lock);
 	spin_lock_irqsave(&priv->driver_lock, flags);
 
@@ -640,30 +560,57 @@ int lbs_process_rx_command(struct lbs_private *priv)
 		spin_unlock_irqrestore(&priv->driver_lock, flags);
 		goto done;
 	}
-	resp = priv->cur_cmd->cmdbuf;
+
+	resp = (void *)priv->upld_buf;
+
+	curcmd = le16_to_cpu(resp->command);
 
 	respcmd = le16_to_cpu(resp->command);
 	result = le16_to_cpu(resp->result);
 
-	lbs_deb_host("CMD_RESP: response 0x%04x, size %d, jiffies %lu\n",
-		respcmd, priv->upld_len, jiffies);
+	lbs_deb_host("CMD_RESP: response 0x%04x, seq %d, size %d, jiffies %lu\n",
+		     respcmd, le16_to_cpu(resp->seqnum), priv->upld_len, jiffies);
 	lbs_deb_hex(LBS_DEB_HOST, "CMD_RESP", (void *) resp, priv->upld_len);
 
-	if (!(respcmd & 0x8000)) {
-		lbs_deb_host("invalid response!\n");
-		priv->cur_cmd_retcode = -1;
-		__lbs_cleanup_and_insert_cmd(priv, priv->cur_cmd);
-		priv->cur_cmd = NULL;
+	if (resp->seqnum != resp->seqnum) {
+		lbs_pr_info("Received CMD_RESP with invalid sequence %d (expected %d)\n",
+			    le16_to_cpu(resp->seqnum), le16_to_cpu(resp->seqnum));
+		spin_unlock_irqrestore(&priv->driver_lock, flags);
+		ret = -1;
+		goto done;
+	}
+	if (respcmd != CMD_RET(curcmd) &&
+	    respcmd != CMD_802_11_ASSOCIATE && curcmd != CMD_RET_802_11_ASSOCIATE) {
+		lbs_pr_info("Invalid CMD_RESP %x to command %x!\n", respcmd, curcmd);
+		spin_unlock_irqrestore(&priv->driver_lock, flags);
+		ret = -1;
+		goto done;
+	}
+
+	if (resp->result == cpu_to_le16(0x0004)) {
+		/* 0x0004 means -EAGAIN. Drop the response, let it time out
+		   and be resubmitted */
+		lbs_pr_info("Firmware returns DEFER to command %x. Will let it time out...\n",
+			    le16_to_cpu(resp->command));
 		spin_unlock_irqrestore(&priv->driver_lock, flags);
 		ret = -1;
 		goto done;
 	}
 
+	/* Now we got response from FW, cancel the command timer */
+	del_timer(&priv->command_timer);
+	priv->cmd_timed_out = 0;
+	if (priv->nr_retries) {
+		lbs_pr_info("Received result %x to command %x after %d retries\n",
+			    result, curcmd, priv->nr_retries);
+		priv->nr_retries = 0;
+	}
+
 	/* Store the response code to cur_cmd_retcode. */
 	priv->cur_cmd_retcode = result;
 
 	if (respcmd == CMD_RET(CMD_802_11_PS_MODE)) {
-		struct cmd_ds_802_11_ps_mode *psmode = (void *) resp;
+		struct cmd_ds_802_11_ps_mode *psmode = (void *) &resp[1];
 		u16 action = le16_to_cpu(psmode->action);
 
 		lbs_deb_host(
@@ -708,8 +655,7 @@ int lbs_process_rx_command(struct lbs_private *priv)
 			lbs_deb_host("CMD_RESP: PS action 0x%X\n", action);
 		}
 
-		__lbs_cleanup_and_insert_cmd(priv, priv->cur_cmd);
-		priv->cur_cmd = NULL;
+		lbs_complete_command(priv, priv->cur_cmd, result);
 		spin_unlock_irqrestore(&priv->driver_lock, flags);
 
 		ret = 0;
@@ -730,9 +676,7 @@ int lbs_process_rx_command(struct lbs_private *priv)
 			break;
 
 		}
-
-		__lbs_cleanup_and_insert_cmd(priv, priv->cur_cmd);
-		priv->cur_cmd = NULL;
+		lbs_complete_command(priv, priv->cur_cmd, result);
 		spin_unlock_irqrestore(&priv->driver_lock, flags);
 
 		ret = -1;
@@ -751,8 +695,7 @@ int lbs_process_rx_command(struct lbs_private *priv)
 
 	if (priv->cur_cmd) {
 		/* Clean up and Put current command back to cmdfreeq */
-		__lbs_cleanup_and_insert_cmd(priv, priv->cur_cmd);
-		priv->cur_cmd = NULL;
+		lbs_complete_command(priv, priv->cur_cmd, result);
 	}
 	spin_unlock_irqrestore(&priv->driver_lock, flags);
 
@@ -762,6 +705,30 @@ done:
 	return ret;
 }
 
+static int lbs_send_confirmwake(struct lbs_private *priv)
+{
+	struct cmd_header *cmd = &priv->lbs_ps_confirm_wake;
+	int ret = 0;
+
+	lbs_deb_enter(LBS_DEB_HOST);
+
+	cmd->command = cpu_to_le16(CMD_802_11_WAKEUP_CONFIRM);
+	cmd->size = cpu_to_le16(sizeof(*cmd));
+	cmd->seqnum = cpu_to_le16(++priv->seqnum);
+	cmd->result = 0;
+
+	lbs_deb_host("SEND_WAKEC_CMD: before download\n");
+
+	lbs_deb_hex(LBS_DEB_HOST, "wake confirm command", (void *)cmd, sizeof(*cmd));
+
+	ret = priv->hw_host_to_card(priv, MVMS_CMD, (void *)cmd, sizeof(*cmd));
+	if (ret)
+		lbs_pr_alert("SEND_WAKEC_CMD: Host to Card failed for Confirm Wake\n");
+
+	lbs_deb_leave_args(LBS_DEB_HOST, "ret %d", ret);
+	return ret;
+}
+
 int lbs_process_event(struct lbs_private *priv)
 {
 	int ret = 0;
@@ -810,9 +777,13 @@ int lbs_process_event(struct lbs_private *priv)
 
 		break;
 
+	case MACREG_INT_CODE_HOST_AWAKE:
+		lbs_deb_cmd("EVENT: HOST_AWAKE\n");
+		lbs_send_confirmwake(priv);
+		break;
+
 	case MACREG_INT_CODE_PS_AWAKE:
 		lbs_deb_cmd("EVENT: awake\n");
-
 		/* handle unexpected PS AWAKE event */
 		if (priv->psstate == PS_STATE_FULL_POWER) {
 			lbs_deb_cmd(
@@ -875,9 +846,10 @@ int lbs_process_event(struct lbs_private *priv)
 		}
 		lbs_pr_info("EVENT: MESH_AUTO_STARTED\n");
 		priv->mesh_connect_status = LBS_CONNECTED;
-		if (priv->mesh_open == 1) {
-			netif_wake_queue(priv->mesh_dev);
+		if (priv->mesh_open) {
 			netif_carrier_on(priv->mesh_dev);
+			if (!priv->tx_pending_len)
+				netif_wake_queue(priv->mesh_dev);
 		}
 		priv->mode = IW_MODE_ADHOC;
 		schedule_work(&priv->sync_channel);
diff --git a/package/libertas/src/debugfs.c b/package/libertas/src/debugfs.c
index f4858bd7d5..2174717e46 100644
--- a/package/libertas/src/debugfs.c
+++ b/package/libertas/src/debugfs.c
@@ -10,6 +10,7 @@
 #include "decl.h"
 #include "host.h"
 #include "debugfs.h"
+#include "cmd.h"
 
 static struct dentry *lbs_dir;
 static char *szStates[] = {
@@ -103,71 +104,64 @@ static ssize_t lbs_sleepparams_write(struct file *file,
 				loff_t *ppos)
 {
 	struct lbs_private *priv = file->private_data;
-	ssize_t buf_size, res;
+	ssize_t buf_size, ret;
+	struct sleep_params sp;
 	int p1, p2, p3, p4, p5, p6;
 	unsigned long addr = get_zeroed_page(GFP_KERNEL);
 	char *buf = (char *)addr;
 
 	buf_size = min(count, len - 1);
 	if (copy_from_user(buf, user_buf, buf_size)) {
-		res = -EFAULT;
+		ret = -EFAULT;
 		goto out_unlock;
 	}
-	res = sscanf(buf, "%d %d %d %d %d %d", &p1, &p2, &p3, &p4, &p5, &p6);
-	if (res != 6) {
-		res = -EFAULT;
+	ret = sscanf(buf, "%d %d %d %d %d %d", &p1, &p2, &p3, &p4, &p5, &p6);
+	if (ret != 6) {
+		ret = -EINVAL;
 		goto out_unlock;
 	}
-	priv->sp.sp_error = p1;
-	priv->sp.sp_offset = p2;
-	priv->sp.sp_stabletime = p3;
-	priv->sp.sp_calcontrol = p4;
-	priv->sp.sp_extsleepclk = p5;
-	priv->sp.sp_reserved = p6;
-
-	res = lbs_prepare_and_send_command(priv,
-				CMD_802_11_SLEEP_PARAMS,
-				CMD_ACT_SET,
-				CMD_OPTION_WAITFORRSP, 0, NULL);
-
-	if (!res)
-		res = count;
-	else
-		res = -EINVAL;
+	sp.sp_error = p1;
+	sp.sp_offset = p2;
+	sp.sp_stabletime = p3;
+	sp.sp_calcontrol = p4;
+	sp.sp_extsleepclk = p5;
+	sp.sp_reserved = p6;
+
+	ret = lbs_cmd_802_11_sleep_params(priv, CMD_ACT_SET, &sp);
+	if (!ret)
+		ret = count;
+	else if (ret > 0)
+		ret = -EINVAL;
 
 out_unlock:
 	free_page(addr);
-	return res;
+	return ret;
 }
 
 static ssize_t lbs_sleepparams_read(struct file *file, char __user *userbuf,
 				  size_t count, loff_t *ppos)
 {
 	struct lbs_private *priv = file->private_data;
-	ssize_t res;
+	ssize_t ret;
 	size_t pos = 0;
+	struct sleep_params sp;
 	unsigned long addr = get_zeroed_page(GFP_KERNEL);
 	char *buf = (char *)addr;
 
-	res = lbs_prepare_and_send_command(priv,
-				CMD_802_11_SLEEP_PARAMS,
-				CMD_ACT_GET,
-				CMD_OPTION_WAITFORRSP, 0, NULL);
-	if (res) {
-		res = -EFAULT;
+	ret = lbs_cmd_802_11_sleep_params(priv, CMD_ACT_GET, &sp);
+	if (ret)
 		goto out_unlock;
-	}
 
-	pos += snprintf(buf, len, "%d %d %d %d %d %d\n", priv->sp.sp_error,
-			priv->sp.sp_offset, priv->sp.sp_stabletime,
-			priv->sp.sp_calcontrol, priv->sp.sp_extsleepclk,
-			priv->sp.sp_reserved);
+	pos += snprintf(buf, len, "%d %d %d %d %d %d\n", sp.sp_error,
+			sp.sp_offset, sp.sp_stabletime,
+			sp.sp_calcontrol, sp.sp_extsleepclk,
+			sp.sp_reserved);
 
-	res = simple_read_from_buffer(userbuf, count, ppos, buf, pos);
+	ret = simple_read_from_buffer(userbuf, count, ppos, buf, pos);
 
 out_unlock:
 	free_page(addr);
-	return res;
+	return ret;
 }
 
 static ssize_t lbs_extscan(struct file *file, const char __user *userbuf,
@@ -295,7 +289,7 @@ static ssize_t lbs_setuserscan(struct file *file,
 
 	if (!buf)
 		return -ENOMEM;
-		
+
 	buf_size = min(count, len - 1);
 	if (copy_from_user(buf, userbuf, buf_size)) {
 		res = -EFAULT;
@@ -352,20 +346,19 @@ static ssize_t lbs_setuserscan(struct file *file,
  * and returns a pointer to the first data byte of the TLV, or to NULL
  * if the TLV hasn't been found.
  */
-static void *lbs_tlv_find(u16 tlv_type, const u8 *tlv, u16 size)
+static void *lbs_tlv_find(uint16_t tlv_type, const uint8_t *tlv, uint16_t size)
 {
-	__le16 le_type = cpu_to_le16(tlv_type);
-	ssize_t pos = 0;
 	struct mrvlietypesheader *tlv_h;
+	uint16_t length;
+	ssize_t pos = 0;
+
 	while (pos < size) {
-		u16 length;
 		tlv_h = (struct mrvlietypesheader *) tlv;
-		if (tlv_h->type == le_type)
-			return tlv_h;
-		if (tlv_h->len == 0)
+		if (!tlv_h->len)
 			return NULL;
-		length = le16_to_cpu(tlv_h->len) +
-			sizeof(struct mrvlietypesheader);
+		if (tlv_h->type == cpu_to_le16(tlv_type))
+			return tlv_h;
+		length = le16_to_cpu(tlv_h->len) + sizeof(*tlv_h);
 		pos += length;
 		tlv += length;
 	}
@@ -373,100 +366,100 @@ static void *lbs_tlv_find(u16 tlv_type, const u8 *tlv, u16 size)
 }
 
 
-/*
- * This just gets the bitmap of currently subscribed events. Used when
- * adding an additonal event subscription.
- */
-static u16 lbs_get_events_bitmap(struct lbs_private *priv)
-{
-	ssize_t res;
-
-	struct cmd_ds_802_11_subscribe_event *events = kzalloc(
-		sizeof(struct cmd_ds_802_11_subscribe_event),
-		GFP_KERNEL);
-
-	res = lbs_prepare_and_send_command(priv,
-			CMD_802_11_SUBSCRIBE_EVENT, CMD_ACT_GET,
-			CMD_OPTION_WAITFORRSP, 0, events);
-
-	if (res) {
-		kfree(events);
-		return 0;
-	}
-	return le16_to_cpu(events->events);
-}
-
-
-static ssize_t lbs_threshold_read(
-	u16 tlv_type, u16 event_mask,
-	struct file *file, char __user *userbuf,
-	size_t count, loff_t *ppos)
+static ssize_t lbs_threshold_read(uint16_t tlv_type, uint16_t event_mask,
+				  struct file *file, char __user *userbuf,
+				  size_t count, loff_t *ppos)
 {
+	struct cmd_ds_802_11_subscribe_event *subscribed;
+	struct mrvlietypes_thresholds *got;
 	struct lbs_private *priv = file->private_data;
-	ssize_t res = 0;
+	ssize_t ret = 0;
 	size_t pos = 0;
-	unsigned long addr = get_zeroed_page(GFP_KERNEL);
-	char *buf = (char *)addr;
+	char *buf;
 	u8 value;
 	u8 freq;
 	int events = 0;
 
-	struct cmd_ds_802_11_subscribe_event *subscribed = kzalloc(
-		sizeof(struct cmd_ds_802_11_subscribe_event),
-		GFP_KERNEL);
-	struct mrvlietypes_thresholds *got;
+	buf = (char *)get_zeroed_page(GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
 
-	res = lbs_prepare_and_send_command(priv,
-			CMD_802_11_SUBSCRIBE_EVENT, CMD_ACT_GET,
-			CMD_OPTION_WAITFORRSP, 0, subscribed);
-	if (res) {
-		kfree(subscribed);
-		return res;
+	subscribed = kzalloc(sizeof(*subscribed), GFP_KERNEL);
+	if (!subscribed) {
+		ret = -ENOMEM;
+		goto out_page;
 	}
 
+	subscribed->hdr.size = cpu_to_le16(sizeof(*subscribed));
+	subscribed->action = cpu_to_le16(CMD_ACT_GET);
+
+	ret = lbs_cmd_with_response(priv, CMD_802_11_SUBSCRIBE_EVENT, subscribed);
+	if (ret)
+		goto out_cmd;
+
 	got = lbs_tlv_find(tlv_type, subscribed->tlv, sizeof(subscribed->tlv));
 	if (got) {
 		value = got->value;
 		freq  = got->freq;
 		events = le16_to_cpu(subscribed->events);
-	}
-	kfree(subscribed);
 
-	if (got)
 		pos += snprintf(buf, len, "%d %d %d\n", value, freq,
-			!!(events & event_mask));
+				!!(events & event_mask));
+	}
 
-	res = simple_read_from_buffer(userbuf, count, ppos, buf, pos);
+	ret = simple_read_from_buffer(userbuf, count, ppos, buf, pos);
 
-	free_page(addr);
-	return res;
+ out_cmd:
+	kfree(subscribed);
+
+ out_page:
+	free_page((unsigned long)buf);
+	return ret;
 }
 
 
-static ssize_t lbs_threshold_write(
-	u16 tlv_type, u16 event_mask,
-	struct file *file,
-	const char __user *userbuf,
-	size_t count, loff_t *ppos)
+static ssize_t lbs_threshold_write(uint16_t tlv_type, uint16_t event_mask,
+				   struct file *file,
+				   const char __user *userbuf, size_t count,
+				   loff_t *ppos)
 {
-	struct lbs_private *priv = file->private_data;
-	ssize_t res, buf_size;
-	int value, freq, curr_mask, new_mask;
-	unsigned long addr = get_zeroed_page(GFP_KERNEL);
-	char *buf = (char *)addr;
 	struct cmd_ds_802_11_subscribe_event *events;
+	struct mrvlietypes_thresholds *tlv;
+	struct lbs_private *priv = file->private_data;
+	ssize_t buf_size;
+	int value, freq, new_mask;
+	uint16_t curr_mask;
+	char *buf;
+	int ret;
+
+	buf = (char *)get_zeroed_page(GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
 
 	buf_size = min(count, len - 1);
 	if (copy_from_user(buf, userbuf, buf_size)) {
-		res = -EFAULT;
-		goto out_unlock;
+		ret = -EFAULT;
+		goto out_page;
 	}
-	res = sscanf(buf, "%d %d %d", &value, &freq, &new_mask);
-	if (res != 3) {
-		res = -EFAULT;
-		goto out_unlock;
+	ret = sscanf(buf, "%d %d %d", &value, &freq, &new_mask);
+	if (ret != 3) {
+		ret = -EINVAL;
+		goto out_page;
 	}
-	curr_mask = lbs_get_events_bitmap(priv);
+	events = kzalloc(sizeof(*events), GFP_KERNEL);
+	if (!events) {
+		ret = -ENOMEM;
+		goto out_page;
+	}
+
+	events->hdr.size = cpu_to_le16(sizeof(*events));
+	events->action = cpu_to_le16(CMD_ACT_GET);
+
+	ret = lbs_cmd_with_response(priv, CMD_802_11_SUBSCRIBE_EVENT, events);
+	if (ret)
+		goto out_events;
+
+	curr_mask = le16_to_cpu(events->events);
 
 	if (new_mask)
 		new_mask = curr_mask | event_mask;
@@ -474,147 +467,128 @@ static ssize_t lbs_threshold_write(
 		new_mask = curr_mask & ~event_mask;
 
 	/* Now everything is set and we can send stuff down to the firmware */
-	events = kzalloc(
-		sizeof(struct cmd_ds_802_11_subscribe_event),
-		GFP_KERNEL);
-	if (events) {
-		struct mrvlietypes_thresholds *tlv =
-			(struct mrvlietypes_thresholds *) events->tlv;
-		events->action = cpu_to_le16(CMD_ACT_SET);
-		events->events = cpu_to_le16(new_mask);
-		tlv->header.type = cpu_to_le16(tlv_type);
-		tlv->header.len = cpu_to_le16(
-			sizeof(struct mrvlietypes_thresholds) -
-			sizeof(struct mrvlietypesheader));
-		tlv->value = value;
-		if (tlv_type != TLV_TYPE_BCNMISS)
-			tlv->freq = freq;
-		lbs_prepare_and_send_command(priv,
-			CMD_802_11_SUBSCRIBE_EVENT, CMD_ACT_SET,
-			CMD_OPTION_WAITFORRSP, 0, events);
-		kfree(events);
-	}
 
-	res = count;
-out_unlock:
-	free_page(addr);
-	return res;
+	tlv = (void *)events->tlv;
+
+	events->action = cpu_to_le16(CMD_ACT_SET);
+	events->events = cpu_to_le16(new_mask);
+	tlv->header.type = cpu_to_le16(tlv_type);
+	tlv->header.len = cpu_to_le16(sizeof(*tlv) - sizeof(tlv->header));
+	tlv->value = value;
+	if (tlv_type != TLV_TYPE_BCNMISS)
+		tlv->freq = freq;
+
+	/* The command header, the event mask, and the one TLV */
+	events->hdr.size = cpu_to_le16(sizeof(events->hdr) + 2 + sizeof(*tlv));
+
+	ret = lbs_cmd_with_response(priv, CMD_802_11_SUBSCRIBE_EVENT, events);
+
+	if (!ret)
+		ret = count;
+ out_events:
+	kfree(events);
+ out_page:
+	free_page((unsigned long)buf);
+	return ret;
 }
 
 
-static ssize_t lbs_lowrssi_read(
-	struct file *file, char __user *userbuf,
-	size_t count, loff_t *ppos)
+static ssize_t lbs_lowrssi_read(struct file *file, char __user *userbuf,
+				size_t count, loff_t *ppos)
 {
 	return lbs_threshold_read(TLV_TYPE_RSSI_LOW, CMD_SUBSCRIBE_RSSI_LOW,
-		file, userbuf, count, ppos);
+				  file, userbuf, count, ppos);
 }
 
 
-static ssize_t lbs_lowrssi_write(
-	struct file *file, const char __user *userbuf,
-	size_t count, loff_t *ppos)
+static ssize_t lbs_lowrssi_write(struct file *file, const char __user *userbuf,
+				 size_t count, loff_t *ppos)
 {
 	return lbs_threshold_write(TLV_TYPE_RSSI_LOW, CMD_SUBSCRIBE_RSSI_LOW,
-		file, userbuf, count, ppos);
+				   file, userbuf, count, ppos);
 }
 
 
-static ssize_t lbs_lowsnr_read(
-	struct file *file, char __user *userbuf,
-	size_t count, loff_t *ppos)
+static ssize_t lbs_lowsnr_read(struct file *file, char __user *userbuf,
+			       size_t count, loff_t *ppos)
 {
 	return lbs_threshold_read(TLV_TYPE_SNR_LOW, CMD_SUBSCRIBE_SNR_LOW,
-		file, userbuf, count, ppos);
+				  file, userbuf, count, ppos);
 }
 
 
-static ssize_t lbs_lowsnr_write(
-	struct file *file, const char __user *userbuf,
-	size_t count, loff_t *ppos)
+static ssize_t lbs_lowsnr_write(struct file *file, const char __user *userbuf,
+				size_t count, loff_t *ppos)
 {
 	return lbs_threshold_write(TLV_TYPE_SNR_LOW, CMD_SUBSCRIBE_SNR_LOW,
-		file, userbuf, count, ppos);
+				   file, userbuf, count, ppos);
 }
 
 
-static ssize_t lbs_failcount_read(
-	struct file *file, char __user *userbuf,
-	size_t count, loff_t *ppos)
+static ssize_t lbs_failcount_read(struct file *file, char __user *userbuf,
+				  size_t count, loff_t *ppos)
 {
 	return lbs_threshold_read(TLV_TYPE_FAILCOUNT, CMD_SUBSCRIBE_FAILCOUNT,
-		file, userbuf, count, ppos);
+				  file, userbuf, count, ppos);
 }
 
 
-static ssize_t lbs_failcount_write(
-	struct file *file, const char __user *userbuf,
-	size_t count, loff_t *ppos)
+static ssize_t lbs_failcount_write(struct file *file, const char __user *userbuf,
+				   size_t count, loff_t *ppos)
 {
 	return lbs_threshold_write(TLV_TYPE_FAILCOUNT, CMD_SUBSCRIBE_FAILCOUNT,
-		file, userbuf, count, ppos);
+				   file, userbuf, count, ppos);
 }
 
 
-static ssize_t lbs_highrssi_read(
-	struct file *file, char __user *userbuf,
-	size_t count, loff_t *ppos)
+static ssize_t lbs_highrssi_read(struct file *file, char __user *userbuf,
+				 size_t count, loff_t *ppos)
 {
 	return lbs_threshold_read(TLV_TYPE_RSSI_HIGH, CMD_SUBSCRIBE_RSSI_HIGH,
-		file, userbuf, count, ppos);
+				  file, userbuf, count, ppos);
 }
 
 
-static ssize_t lbs_highrssi_write(
-	struct file *file, const char __user *userbuf,
-	size_t count, loff_t *ppos)
+static ssize_t lbs_highrssi_write(struct file *file, const char __user *userbuf,
+				  size_t count, loff_t *ppos)
 {
 	return lbs_threshold_write(TLV_TYPE_RSSI_HIGH, CMD_SUBSCRIBE_RSSI_HIGH,
-		file, userbuf, count, ppos);
+				   file, userbuf, count, ppos);
 }
 
 
-static ssize_t lbs_highsnr_read(
-	struct file *file, char __user *userbuf,
-	size_t count, loff_t *ppos)
+static ssize_t lbs_highsnr_read(struct file *file, char __user *userbuf,
+				size_t count, loff_t *ppos)
 {
 	return lbs_threshold_read(TLV_TYPE_SNR_HIGH, CMD_SUBSCRIBE_SNR_HIGH,
-		file, userbuf, count, ppos);
+				  file, userbuf, count, ppos);
 }
 
 
-static ssize_t lbs_highsnr_write(
-	struct file *file, const char __user *userbuf,
-	size_t count, loff_t *ppos)
+static ssize_t lbs_highsnr_write(struct file *file, const char __user *userbuf,
+				 size_t count, loff_t *ppos)
 {
 	return lbs_threshold_write(TLV_TYPE_SNR_HIGH, CMD_SUBSCRIBE_SNR_HIGH,
-		file, userbuf, count, ppos);
+				   file, userbuf, count, ppos);
 }
 
-static ssize_t lbs_bcnmiss_read(
-	struct file *file, char __user *userbuf,
-	size_t count, loff_t *ppos)
+static ssize_t lbs_bcnmiss_read(struct file *file, char __user *userbuf,
+				size_t count, loff_t *ppos)
 {
 	return lbs_threshold_read(TLV_TYPE_BCNMISS, CMD_SUBSCRIBE_BCNMISS,
-		file, userbuf, count, ppos);
+				  file, userbuf, count, ppos);
 }
 
 
-static ssize_t lbs_bcnmiss_write(
-	struct file *file, const char __user *userbuf,
-	size_t count, loff_t *ppos)
+static ssize_t lbs_bcnmiss_write(struct file *file, const char __user *userbuf,
+				 size_t count, loff_t *ppos)
 {
 	return lbs_threshold_write(TLV_TYPE_BCNMISS, CMD_SUBSCRIBE_BCNMISS,
-		file, userbuf, count, ppos);
+				   file, userbuf, count, ppos);
 }
 
 
 
-
-
-
-
-
 static ssize_t lbs_rdmac_read(struct file *file, char __user *userbuf,
 				  size_t count, loff_t *ppos)
 {
diff --git a/package/libertas/src/decl.h b/package/libertas/src/decl.h
index 9b0ef16618..aaacd9bd6b 100644
--- a/package/libertas/src/decl.h
+++ b/package/libertas/src/decl.h
@@ -28,10 +28,6 @@ int lbs_prepare_and_send_command(struct lbs_private *priv,
 	u16 cmd_action,
 	u16 wait_option, u32 cmd_oid, void *pdata_buf);
 
-void lbs_queue_cmd(struct lbs_private *priv,
-	struct cmd_ctrl_node *cmdnode,
-	u8 addtail);
-
 int lbs_allocate_cmd_buffer(struct lbs_private *priv);
 int lbs_execute_next_command(struct lbs_private *priv);
 int lbs_process_event(struct lbs_private *priv);
@@ -45,9 +41,8 @@ void lbs_get_fwversion(struct lbs_private *priv,
 
 /** The proc fs interface */
 int lbs_process_rx_command(struct lbs_private *priv);
-void __lbs_cleanup_and_insert_cmd(struct lbs_private *priv,
-	struct cmd_ctrl_node *ptempcmd);
-
+void lbs_complete_command(struct lbs_private *priv, struct cmd_ctrl_node *cmd,
+			  int result);
 int lbs_hard_start_xmit(struct sk_buff *skb, struct net_device *dev);
 int lbs_set_regiontable(struct lbs_private *priv, u8 region, u8 band);
 
@@ -77,4 +72,5 @@ int lbs_stop_card(struct lbs_private *priv);
 int lbs_reset_device(struct lbs_private *priv);
 void lbs_host_to_card_done(struct lbs_private *priv);
 
+int lbs_update_channel(struct lbs_private *priv);
 #endif
diff --git a/package/libertas/src/defs.h b/package/libertas/src/defs.h
index 9b98ae720b..3053cc2160 100644
--- a/package/libertas/src/defs.h
+++ b/package/libertas/src/defs.h
@@ -141,6 +141,13 @@ static inline void lbs_deb_hex(unsigned int grp, const char *prompt, u8 *buf, in
 #define	LBS_UPLD_SIZE			2312
 #define DEV_NAME_LEN			32
 
+/* Wake criteria for HOST_SLEEP_CFG command */
+#define EHS_WAKE_ON_BROADCAST_DATA	0x0001
+#define EHS_WAKE_ON_UNICAST_DATA	0x0002
+#define EHS_WAKE_ON_MAC_EVENT		0x0004
+#define EHS_WAKE_ON_MULTICAST_DATA	0x0008
+#define EHS_REMOVE_WAKEUP		0xFFFFFFFF
+
 /** Misc constants */
 /* This section defines 802.11 specific contants */
 
diff --git a/package/libertas/src/dev.h b/package/libertas/src/dev.h
index 86b45a471f..362975189e 100644
--- a/package/libertas/src/dev.h
+++ b/package/libertas/src/dev.h
@@ -77,12 +77,12 @@ struct current_bss_params {
 
 /** sleep_params */
 struct sleep_params {
-	u16 sp_error;
-	u16 sp_offset;
-	u16 sp_stabletime;
-	u8 sp_calcontrol;
-	u8 sp_extsleepclk;
-	u16 sp_reserved;
+	uint16_t sp_error;
+	uint16_t sp_offset;
+	uint16_t sp_stabletime;
+	uint8_t  sp_calcontrol;
+	uint8_t  sp_extsleepclk;
+	uint16_t sp_reserved;
 };
 
 /* Mesh statistics */
@@ -153,6 +153,11 @@ struct lbs_private {
 	int (*hw_get_int_status) (struct lbs_private *priv, u8 *);
 	int (*hw_read_event_cause) (struct lbs_private *);
 
+	/* Wake On LAN */
+	uint32_t wol_criteria;
+	uint8_t wol_gpio;
+	uint8_t wol_gap;
+
 	/* was struct lbs_adapter from here... */
 
 	/** Wlan adapter data structure*/
@@ -196,11 +201,15 @@ struct lbs_private {
 
 	/** Timers */
 	struct timer_list command_timer;
+	int nr_retries;
+	int cmd_timed_out;
 
 	u8 hisregcpy;
 
 	/** current ssid/bssid related parameters*/
 	struct current_bss_params curbssparams;
+
+	uint16_t mesh_tlv;
 	u8 mesh_ssid[IW_ESSID_MAX_SIZE + 1];
 	u8 mesh_ssid_len;
 
@@ -251,9 +260,11 @@ struct lbs_private {
 	u16 psmode;		/* Wlan802_11PowermodeCAM=disable
 				   Wlan802_11PowermodeMAX_PSP=enable */
 	u32 psstate;
+	char ps_supported;
 	u8 needtowakeup;
 
 	struct PS_CMD_ConfirmSleep lbs_ps_confirm_sleep;
+	struct cmd_header lbs_ps_confirm_wake;
 
 	struct assoc_request * pending_assoc_req;
 	struct assoc_request * in_progress_assoc_req;
@@ -289,9 +300,6 @@ struct lbs_private {
 	u8 cur_rate;
 	u8 auto_rate;
 
-	/** sleep_params */
-	struct sleep_params sp;
-
 	/** RF calibration data */
 
 #define	MAX_REGION_CHANNEL_NUM	2
diff --git a/package/libertas/src/ethtool.c b/package/libertas/src/ethtool.c
index 98757f1e81..21e6f988ea 100644
--- a/package/libertas/src/ethtool.c
+++ b/package/libertas/src/ethtool.c
@@ -8,6 +8,8 @@
 #include "dev.h"
 #include "join.h"
 #include "wext.h"
+#include "cmd.h"
+
 static const char * mesh_stat_strings[]= {
 			"drop_duplicate_bcast",
 			"drop_ttl_zero",
@@ -142,6 +144,16 @@ static void lbs_ethtool_get_stats(struct net_device * dev,
 	lbs_deb_enter(LBS_DEB_ETHTOOL);
 }
 
+static int lbs_ethtool_get_sset_count(struct net_device * dev, int sset)
+{
+	switch (sset) {
+	case ETH_SS_STATS:
+		return MESH_STATS_NUM;
+	default:
+		return -EOPNOTSUPP;
+	}
+}
+
 static void lbs_ethtool_get_strings(struct net_device *dev,
 					  u32 stringset,
 					  u8 * s)
@@ -162,11 +174,57 @@ static void lbs_ethtool_get_strings(struct net_device *dev,
 	lbs_deb_enter(LBS_DEB_ETHTOOL);
 }
 
+static void lbs_ethtool_get_wol(struct net_device *dev,
+				struct ethtool_wolinfo *wol)
+{
+	struct lbs_private *priv = dev->priv;
+
+	if (priv->wol_criteria == 0xffffffff) {
+		/* Interface driver didn't configure wake */
+		wol->supported = wol->wolopts = 0;
+		return;
+	}
+
+	wol->supported = WAKE_UCAST|WAKE_MCAST|WAKE_BCAST|WAKE_PHY;
+
+	if (priv->wol_criteria & EHS_WAKE_ON_UNICAST_DATA)
+		wol->wolopts |= WAKE_UCAST;
+	if (priv->wol_criteria & EHS_WAKE_ON_MULTICAST_DATA)
+		wol->wolopts |= WAKE_MCAST;
+	if (priv->wol_criteria & EHS_WAKE_ON_BROADCAST_DATA)
+		wol->wolopts |= WAKE_BCAST;
+	if (priv->wol_criteria & EHS_WAKE_ON_MAC_EVENT)
+		wol->wolopts |= WAKE_PHY;
+}
+
+static int lbs_ethtool_set_wol(struct net_device *dev,
+			       struct ethtool_wolinfo *wol)
+{
+	struct lbs_private *priv = dev->priv;
+	uint32_t criteria = 0;
+
+	if (priv->wol_criteria == 0xffffffff && wol->wolopts)
+		return -EOPNOTSUPP;
+
+	if (wol->wolopts & ~(WAKE_UCAST|WAKE_MCAST|WAKE_BCAST|WAKE_PHY))
+		return -EOPNOTSUPP;
+
+	if (wol->wolopts & WAKE_UCAST) criteria |= EHS_WAKE_ON_UNICAST_DATA;
+	if (wol->wolopts & WAKE_MCAST) criteria |= EHS_WAKE_ON_MULTICAST_DATA;
+	if (wol->wolopts & WAKE_BCAST) criteria |= EHS_WAKE_ON_BROADCAST_DATA;
+	if (wol->wolopts & WAKE_PHY)   criteria |= EHS_WAKE_ON_MAC_EVENT;
+
+	return lbs_host_sleep_cfg(priv, criteria);
+}
+
 struct ethtool_ops lbs_ethtool_ops = {
 	.get_drvinfo = lbs_ethtool_get_drvinfo,
 	.get_eeprom =  lbs_ethtool_get_eeprom,
 	.get_eeprom_len = lbs_ethtool_get_eeprom_len,
+	.get_sset_count = lbs_ethtool_get_sset_count,
 	.get_ethtool_stats = lbs_ethtool_get_stats,
 	.get_strings = lbs_ethtool_get_strings,
+	.get_wol = lbs_ethtool_get_wol,
+	.set_wol = lbs_ethtool_set_wol,
 };
 
diff --git a/package/libertas/src/host.h b/package/libertas/src/host.h
index 64178cff2f..1aa04076b1 100644
--- a/package/libertas/src/host.h
+++ b/package/libertas/src/host.h
@@ -73,6 +73,9 @@
 #define CMD_802_11_SET_AFC			0x003c
 #define CMD_802_11_GET_AFC			0x003d
 #define CMD_802_11_AD_HOC_STOP			0x0040
+#define CMD_802_11_HOST_SLEEP_CFG		0x0043
+#define CMD_802_11_WAKEUP_CONFIRM		0x0044
+#define CMD_802_11_HOST_SLEEP_ACTIVATE		0x0045
 #define CMD_802_11_BEACON_STOP			0x0049
 #define CMD_802_11_MAC_ADDRESS			0x004d
 #define CMD_802_11_LED_GPIO_CTRL		0x004e
@@ -82,8 +85,10 @@
 #define CMD_802_11_KEY_MATERIAL			0x005e
 #define CMD_802_11_SLEEP_PARAMS			0x0066
 #define CMD_802_11_INACTIVITY_TIMEOUT		0x0067
+#define CMD_802_11_SLEEP_PERIOD			0x0068
 #define CMD_802_11_TPC_CFG			0x0072
 #define CMD_802_11_PWR_CFG			0x0073
+#define CMD_802_11_FW_WAKE_METHOD		0x0074
 #define CMD_802_11_SUBSCRIBE_EVENT		0x0075
 #define CMD_802_11_RATE_ADAPT_RATESET		0x0076
 #define CMD_802_11_TX_RATE_QUERY		0x007f
@@ -204,6 +209,11 @@
 #define	CMD_TYPE_MAX_PSP		0x0001
 #define CMD_TYPE_FAST_PSP		0x0002
 
+/* Options for CMD_802_11_FW_WAKE_METHOD */
+#define CMD_WAKE_METHOD_UNCHANGED	0x0000
+#define CMD_WAKE_METHOD_COMMAND_INT	0x0001
+#define CMD_WAKE_METHOD_GPIO		0x0002
+
 /* Define action or option for CMD_BT_ACCESS */
 enum cmd_bt_access_opts {
 	/* The bt commands start at 5 instead of 1 because the old dft commands
diff --git a/package/libertas/src/hostcmd.h b/package/libertas/src/hostcmd.h
index aab5d64f32..be325eda6a 100644
--- a/package/libertas/src/hostcmd.h
+++ b/package/libertas/src/hostcmd.h
@@ -74,10 +74,8 @@ struct cmd_header {
 
 struct cmd_ctrl_node {
 	struct list_head list;
-	/* wait for finish or not */
-	u16 wait_option;
+	int result;
 	/* command response */
-	void *pdata_buf;
 	int (*callback)(struct lbs_private *, unsigned long, struct cmd_header *);
 	unsigned long callback_arg;
 	/* command data */
@@ -158,6 +156,8 @@ struct cmd_ds_802_11_reset {
 };
 
 struct cmd_ds_802_11_subscribe_event {
+	struct cmd_header hdr;
+
 	__le16 action;
 	__le16 events;
 
@@ -166,7 +166,7 @@ struct cmd_ds_802_11_subscribe_event {
 	 * 40 bytes. However, future firmware might add additional TLVs, so I
 	 * bump this up a bit.
 	 */
-	u8 tlv[128];
+	uint8_t tlv[128];
 };
 
 /*
@@ -258,6 +258,8 @@ struct cmd_ds_802_11_ad_hoc_result {
 };
 
 struct cmd_ds_802_11_set_wep {
+	struct cmd_header hdr;
+
 	/* ACT_ADD, ACT_REMOVE or ACT_ENABLE */
 	__le16 action;
 
@@ -265,8 +267,8 @@ struct cmd_ds_802_11_set_wep {
 	__le16 keyindex;
 
 	/* 40, 128bit or TXWEP */
-	u8 keytype[4];
-	u8 keymaterial[4][16];
+	uint8_t keytype[4];
+	uint8_t keymaterial[4][16];
 };
 
 struct cmd_ds_802_3_get_stat {
@@ -344,6 +346,8 @@ struct cmd_ds_rf_reg_access {
 };
 
 struct cmd_ds_802_11_radio_control {
+	struct cmd_header hdr;
+
 	__le16 action;
 	__le16 control;
 };
@@ -355,6 +359,8 @@ struct cmd_ds_802_11_beacon_control {
 };
 
 struct cmd_ds_802_11_sleep_params {
+	struct cmd_header hdr;
+
 	/* ACT_GET/ACT_SET */
 	__le16 action;
 
@@ -368,16 +374,18 @@ struct cmd_ds_802_11_sleep_params {
 	__le16 stabletime;
 
 	/* control periodic calibration */
-	u8 calcontrol;
+	uint8_t calcontrol;
 
 	/* control the use of external sleep clock */
-	u8 externalsleepclk;
+	uint8_t externalsleepclk;
 
 	/* reserved field, should be set to zero */
 	__le16 reserved;
 };
 
 struct cmd_ds_802_11_inactivity_timeout {
+	struct cmd_header hdr;
+
 	/* ACT_GET/ACT_SET */
 	__le16 action;
 
@@ -441,6 +449,20 @@ struct cmd_ds_set_boot2_ver {
 	__le16 version;
 };
 
+struct cmd_ds_802_11_fw_wake_method {
+	struct cmd_header hdr;
+
+	__le16 action;
+	__le16 method;
+};
+
+struct cmd_ds_802_11_sleep_period {
+	struct cmd_header hdr;
+
+	__le16 action;
+	__le16 period;
+};
+
 struct cmd_ds_802_11_ps_mode {
 	__le16 action;
 	__le16 nullpktinterval;
@@ -516,6 +538,8 @@ struct cmd_ds_802_11_ad_hoc_join {
 } __attribute__ ((packed));
 
 struct cmd_ds_802_11_enable_rsn {
+	struct cmd_header hdr;
+
 	__le16 action;
 	__le16 enable;
 } __attribute__ ((packed));
@@ -540,6 +564,13 @@ struct MrvlIEtype_keyParamSet {
 	u8 key[32];
 };
 
+struct cmd_ds_host_sleep {
+	struct cmd_header hdr;
+	__le32 criteria;
+	uint8_t gpio;
+	uint8_t gap;
+} __attribute__ ((packed));
+
 struct cmd_ds_802_11_key_material {
 	__le16 action;
 	struct MrvlIEtype_keyParamSet keyParamSet[2];
@@ -663,7 +694,6 @@ struct cmd_ds_command {
 		struct cmd_ds_mac_control macctrl;
 		struct cmd_ds_802_11_associate associate;
 		struct cmd_ds_802_11_deauthenticate deauth;
-		struct cmd_ds_802_11_set_wep wep;
 		struct cmd_ds_802_11_ad_hoc_start ads;
 		struct cmd_ds_802_11_reset reset;
 		struct cmd_ds_802_11_ad_hoc_result result;
@@ -678,13 +708,10 @@ struct cmd_ds_command {
 		struct cmd_ds_802_11_rate_adapt_rateset rateset;
 		struct cmd_ds_mac_multicast_adr madr;
 		struct cmd_ds_802_11_ad_hoc_join adj;
-		struct cmd_ds_802_11_radio_control radio;
-		struct cmd_ds_802_11_rf_channel rfchannel;
 		struct cmd_ds_802_11_rssi rssi;
 		struct cmd_ds_802_11_rssi_rsp rssirsp;
 		struct cmd_ds_802_11_disassociate dassociate;
 		struct cmd_ds_802_11_mac_address macadd;
-		struct cmd_ds_802_11_enable_rsn enbrsn;
 		struct cmd_ds_802_11_key_material keymaterial;
 		struct cmd_ds_mac_reg_access macreg;
 		struct cmd_ds_bbp_reg_access bbpreg;
@@ -694,8 +721,6 @@ struct cmd_ds_command {
 		struct cmd_ds_802_11d_domain_info domaininfo;
 		struct cmd_ds_802_11d_domain_info domaininforesp;
 
-		struct cmd_ds_802_11_sleep_params sleep_params;
-		struct cmd_ds_802_11_inactivity_timeout inactivity_timeout;
 		struct cmd_ds_802_11_tpc_cfg tpccfg;
 		struct cmd_ds_802_11_pwr_cfg pwrcfg;
 		struct cmd_ds_802_11_afc afc;
@@ -705,7 +730,6 @@ struct cmd_ds_command {
 		struct cmd_ds_bt_access bt;
 		struct cmd_ds_fwt_access fwt;
 		struct cmd_ds_get_tsf gettsf;
-		struct cmd_ds_802_11_subscribe_event subscribe_event;
 		struct cmd_ds_802_11_beacon_control bcn_ctrl;
 	} params;
 } __attribute__ ((packed));
diff --git a/package/libertas/src/if_cs.c b/package/libertas/src/if_cs.c
index 58143637c7..030dbe2593 100644
--- a/package/libertas/src/if_cs.c
+++ b/package/libertas/src/if_cs.c
@@ -243,7 +243,7 @@ static inline void if_cs_disable_ints(struct if_cs_card *card)
 
 static irqreturn_t if_cs_interrupt(int irq, void *data)
 {
-	struct if_cs_card *card = data;
+	struct if_cs_card *card = (struct if_cs_card *)data;
 	u16 int_cause;
 
 	lbs_deb_enter(LBS_DEB_CS);
@@ -647,7 +647,6 @@ static int if_cs_get_int_status(struct lbs_private *priv, u8 *ireg)
 	struct if_cs_card *card = (struct if_cs_card *)priv->card;
 	int ret = 0;
 	u16 int_cause;
-	u8 *cmdbuf;
 	*ireg = 0;
 
 	lbs_deb_enter(LBS_DEB_CS);
@@ -679,14 +678,7 @@ sbi_get_int_status_exit:
 	/* Card has a command result for us */
 	if (*ireg & IF_CS_C_S_CMD_UPLD_RDY) {
 		spin_lock(&priv->driver_lock);
-		if (!priv->cur_cmd) {
-			cmdbuf = priv->upld_buf;
-			priv->hisregcpy &= ~IF_CS_C_S_RX_UPLD_RDY;
-		} else {
-			cmdbuf = (u8 *) priv->cur_cmd->cmdbuf;
-		}
-
-		ret = if_cs_receive_cmdres(priv, cmdbuf, &priv->upld_len);
+		ret = if_cs_receive_cmdres(priv, priv->upld_buf, &priv->upld_len);
 		spin_unlock(&priv->driver_lock);
 		if (ret < 0)
 			lbs_pr_err("could not receive cmd from card\n");
diff --git a/package/libertas/src/if_sdio.c b/package/libertas/src/if_sdio.c
index 85675c5fa4..5a828c7b4c 100644
--- a/package/libertas/src/if_sdio.c
+++ b/package/libertas/src/if_sdio.c
@@ -19,7 +19,7 @@
  * current block size.
  *
  * As SDIO is still new to the kernel, it is unfortunately common with
- * bugs in the host controllers related to that. One such bug is that 
+ * bugs in the host controllers related to that. One such bug is that
  * controllers cannot do transfers that aren't a multiple of 4 bytes.
  * If you don't have time to fix the host controller driver, you can
  * work around the problem by modifying if_sdio_host_to_card() and
@@ -136,12 +136,6 @@ static int if_sdio_handle_cmd(struct if_sdio_card *card,
 
 	spin_lock_irqsave(&card->priv->driver_lock, flags);
 
-	if (!card->priv->cur_cmd) {
-		lbs_deb_sdio("discarding spurious response\n");
-		ret = 0;
-		goto out;
-	}
-
 	if (size > LBS_CMD_BUFFER_SIZE) {
 		lbs_deb_sdio("response packet too large (%d bytes)\n",
 			(int)size);
@@ -149,7 +143,7 @@ static int if_sdio_handle_cmd(struct if_sdio_card *card,
 		goto out;
 	}
 
-	memcpy(card->priv->cur_cmd->cmdbuf, buffer, size);
+	memcpy(card->priv->upld_buf, buffer, size);
 	card->priv->upld_len = size;
 
 	card->int_cause |= MRVDRV_CMD_UPLD_RDY;
diff --git a/package/libertas/src/if_usb.c b/package/libertas/src/if_usb.c
index 02192e8a15..7db8e6c35d 100644
--- a/package/libertas/src/if_usb.c
+++ b/package/libertas/src/if_usb.c
@@ -16,6 +16,9 @@
 #include "cmd.h"
 #include "if_usb.h"
 
+#define INSANEDEBUG	0
+#define lbs_deb_usb2(...) do { if (INSANEDEBUG) lbs_deb_usbd(__VA_ARGS__); } while (0)
+
 #define MESSAGE_HEADER_LEN	4
 
 static char *lbs_fw_name = "usb8388.bin";
@@ -32,17 +35,16 @@ MODULE_DEVICE_TABLE(usb, if_usb_table);
 
 static void if_usb_receive(struct urb *urb);
 static void if_usb_receive_fwload(struct urb *urb);
-static int if_usb_prog_firmware(struct usb_card_rec *cardp);
-static int if_usb_host_to_card(struct lbs_private *priv,
-	u8 type,
-	u8 *payload,
-	u16 nb);
-static int if_usb_get_int_status(struct lbs_private *priv, u8 *);
+static int if_usb_prog_firmware(struct if_usb_card *cardp);
+static int if_usb_host_to_card(struct lbs_private *priv, uint8_t type,
+			       uint8_t *payload, uint16_t nb);
+static int if_usb_get_int_status(struct lbs_private *priv, uint8_t *);
 static int if_usb_read_event_cause(struct lbs_private *);
-static int usb_tx_block(struct usb_card_rec *cardp, u8 *payload, u16 nb);
-static void if_usb_free(struct usb_card_rec *cardp);
-static int if_usb_submit_rx_urb(struct usb_card_rec *cardp);
-static int if_usb_reset_device(struct usb_card_rec *cardp);
+static int usb_tx_block(struct if_usb_card *cardp, uint8_t *payload,
+			uint16_t nb);
+static void if_usb_free(struct if_usb_card *cardp);
+static int if_usb_submit_rx_urb(struct if_usb_card *cardp);
+static int if_usb_reset_device(struct if_usb_card *cardp);
 
 /**
  *  @brief  call back function to handle the status of the URB
@@ -51,18 +53,16 @@ static int if_usb_reset_device(struct usb_card_rec *cardp);
  */
 static void if_usb_write_bulk_callback(struct urb *urb)
 {
-	struct usb_card_rec *cardp = (struct usb_card_rec *) urb->context;
+	struct if_usb_card *cardp = (struct if_usb_card *) urb->context;
 
 	/* handle the transmission complete validations */
 
 	if (urb->status == 0) {
 		struct lbs_private *priv = cardp->priv;
 
-		/*
-		lbs_deb_usbd(&urb->dev->dev, "URB status is successfull\n");
-		lbs_deb_usbd(&urb->dev->dev, "Actual length transmitted %d\n",
-		       urb->actual_length);
-		*/
+		lbs_deb_usb2(&urb->dev->dev, "URB status is successful\n");
+		lbs_deb_usb2(&urb->dev->dev, "Actual length transmitted %d\n",
+			     urb->actual_length);
 
 		/* Used for both firmware TX and regular TX.  priv isn't
 		 * valid at firmware load time.
@@ -79,10 +79,10 @@ static void if_usb_write_bulk_callback(struct urb *urb)
 
 /**
  *  @brief  free tx/rx urb, skb and rx buffer
- *  @param cardp	pointer usb_card_rec
+ *  @param cardp	pointer if_usb_card
  *  @return 	   	N/A
  */
-static void if_usb_free(struct usb_card_rec *cardp)
+static void if_usb_free(struct if_usb_card *cardp)
 {
 	lbs_deb_enter(LBS_DEB_USB);
 
@@ -96,26 +96,47 @@ static void if_usb_free(struct usb_card_rec *cardp)
 	usb_free_urb(cardp->rx_urb);
 	cardp->rx_urb = NULL;
 
-	kfree(cardp->bulk_out_buffer);
-	cardp->bulk_out_buffer = NULL;
+	kfree(cardp->ep_out_buf);
+	cardp->ep_out_buf = NULL;
 
 	lbs_deb_leave(LBS_DEB_USB);
 }
 
-static void if_usb_set_boot2_ver(struct lbs_private *priv)
+static void if_usb_setup_firmware(struct lbs_private *priv)
 {
 	struct cmd_ds_set_boot2_ver b2_cmd;
+	struct cmd_ds_802_11_fw_wake_method wake_method;
 
+	b2_cmd.hdr.size = cpu_to_le16(sizeof(b2_cmd));
 	b2_cmd.action = 0;
 	b2_cmd.version = priv->boot2_version;
 
-	if (lbs_cmd(priv, CMD_SET_BOOT2_VER, b2_cmd, NULL, 0))
+	if (lbs_cmd_with_response(priv, CMD_SET_BOOT2_VER, &b2_cmd))
 		lbs_deb_usb("Setting boot2 version failed\n");
+
+	priv->wol_gpio = 2; /* Wake via GPIO2... */
+	priv->wol_gap = 20; /* ... after 20ms    */
+	lbs_host_sleep_cfg(priv, EHS_WAKE_ON_UNICAST_DATA);
+
+	wake_method.hdr.size = cpu_to_le16(sizeof(wake_method));
+	wake_method.action = cpu_to_le16(CMD_ACT_GET);
+	if (lbs_cmd_with_response(priv, CMD_802_11_FW_WAKE_METHOD, &wake_method)) {
+		lbs_pr_info("Firmware does not seem to support PS mode\n");
+	} else {
+		if (le16_to_cpu(wake_method.method) == CMD_WAKE_METHOD_COMMAND_INT) {
+			lbs_deb_usb("Firmware seems to support PS with wake-via-command\n");
+			priv->ps_supported = 1;
+		} else {
+			/* The versions which boot up this way don't seem to
+			   work even if we set it to the command interrupt */
+			lbs_pr_info("Firmware doesn't wake via command interrupt; disabling PS mode\n");
+		}
+	}
 }
 
 static void if_usb_fw_timeo(unsigned long priv)
 {
-	struct usb_card_rec *cardp = (void *)priv;
+	struct if_usb_card *cardp = (void *)priv;
 
 	if (cardp->fwdnldover) {
 		lbs_deb_usb("Download complete, no event. Assuming success\n");
@@ -125,6 +146,7 @@ static void if_usb_fw_timeo(unsigned long priv)
 	}
 	wake_up(&cardp->fw_wq);
 }
+
 /**
  *  @brief sets the configuration values
  *  @param ifnum	interface number
@@ -138,12 +160,12 @@ static int if_usb_probe(struct usb_interface *intf,
 	struct usb_host_interface *iface_desc;
 	struct usb_endpoint_descriptor *endpoint;
 	struct lbs_private *priv;
-	struct usb_card_rec *cardp;
+	struct if_usb_card *cardp;
 	int i;
 
 	udev = interface_to_usbdev(intf);
 
-	cardp = kzalloc(sizeof(struct usb_card_rec), GFP_KERNEL);
+	cardp = kzalloc(sizeof(struct if_usb_card), GFP_KERNEL);
 	if (!cardp) {
 		lbs_pr_err("Out of memory allocating private data.\n");
 		goto error;
@@ -151,12 +173,12 @@ static int if_usb_probe(struct usb_interface *intf,
 
 	setup_timer(&cardp->fw_timeout, if_usb_fw_timeo, (unsigned long)cardp);
 	init_waitqueue_head(&cardp->fw_wq);
-								     
+
 	cardp->udev = udev;
 	iface_desc = intf->cur_altsetting;
 
 	lbs_deb_usbd(&udev->dev, "bcdUSB = 0x%X bDeviceClass = 0x%X"
-	       " bDeviceSubClass = 0x%X, bDeviceProtocol = 0x%X\n",
+		     " bDeviceSubClass = 0x%X, bDeviceProtocol = 0x%X\n",
 		     le16_to_cpu(udev->descriptor.bcdUSB),
 		     udev->descriptor.bDeviceClass,
 		     udev->descriptor.bDeviceSubClass,
@@ -164,63 +186,42 @@ static int if_usb_probe(struct usb_interface *intf,
 
 	for (i = 0; i < iface_desc->desc.bNumEndpoints; ++i) {
 		endpoint = &iface_desc->endpoint[i].desc;
-		if ((endpoint->bEndpointAddress & USB_ENDPOINT_DIR_MASK)
-		    && ((endpoint->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK) ==
-			USB_ENDPOINT_XFER_BULK)) {
-			/* we found a bulk in endpoint */
-			lbs_deb_usbd(&udev->dev, "Bulk in size is %d\n",
-				     le16_to_cpu(endpoint->wMaxPacketSize));
-			if (!(cardp->rx_urb = usb_alloc_urb(0, GFP_KERNEL))) {
-				lbs_deb_usbd(&udev->dev,
-				       "Rx URB allocation failed\n");
-				goto dealloc;
-			}
-			cardp->bulk_in_size =
-				le16_to_cpu(endpoint->wMaxPacketSize);
-			cardp->bulk_in_endpointAddr =
-			    (endpoint->
-			     bEndpointAddress & USB_ENDPOINT_NUMBER_MASK);
-			lbs_deb_usbd(&udev->dev, "in_endpoint = %d\n",
-			       endpoint->bEndpointAddress);
-		}
+		if (usb_endpoint_is_bulk_in(endpoint)) {
+			cardp->ep_in_size = le16_to_cpu(endpoint->wMaxPacketSize);
+			cardp->ep_in = usb_endpoint_num(endpoint);
 
-		if (((endpoint->
-		      bEndpointAddress & USB_ENDPOINT_DIR_MASK) ==
-		     USB_DIR_OUT)
-		    && ((endpoint->bmAttributes & USB_ENDPOINT_XFERTYPE_MASK) ==
-			USB_ENDPOINT_XFER_BULK)) {
-			/* We found bulk out endpoint */
-			if (!(cardp->tx_urb = usb_alloc_urb(0, GFP_KERNEL))) {
-				lbs_deb_usbd(&udev->dev,
-				       "Tx URB allocation failed\n");
-				goto dealloc;
-			}
+			lbs_deb_usbd(&udev->dev, "in_endpoint = %d\n", cardp->ep_in);
+			lbs_deb_usbd(&udev->dev, "Bulk in size is %d\n", cardp->ep_in_size);
 
-			cardp->bulk_out_size =
-				le16_to_cpu(endpoint->wMaxPacketSize);
-			lbs_deb_usbd(&udev->dev,
-				     "Bulk out size is %d\n",
-				     le16_to_cpu(endpoint->wMaxPacketSize));
-			cardp->bulk_out_endpointAddr =
-			    endpoint->bEndpointAddress;
-			lbs_deb_usbd(&udev->dev, "out_endpoint = %d\n",
-				    endpoint->bEndpointAddress);
-			cardp->bulk_out_buffer =
-			    kmalloc(MRVDRV_ETH_TX_PACKET_BUFFER_SIZE,
-				    GFP_KERNEL);
-
-			if (!cardp->bulk_out_buffer) {
-				lbs_deb_usbd(&udev->dev,
-				       "Could not allocate buffer\n");
-				goto dealloc;
-			}
+		} else if (usb_endpoint_is_bulk_out(endpoint)) {
+			cardp->ep_out_size = le16_to_cpu(endpoint->wMaxPacketSize);
+			cardp->ep_out = usb_endpoint_num(endpoint);
+
+			lbs_deb_usbd(&udev->dev, "out_endpoint = %d\n", cardp->ep_out);
+			lbs_deb_usbd(&udev->dev, "Bulk out size is %d\n", cardp->ep_out_size);
 		}
 	}
+	if (!cardp->ep_out_size || !cardp->ep_in_size) {
+		lbs_deb_usbd(&udev->dev, "Endpoints not found\n");
+		goto dealloc;
+	}
+	if (!(cardp->rx_urb = usb_alloc_urb(0, GFP_KERNEL))) {
+		lbs_deb_usbd(&udev->dev, "Rx URB allocation failed\n");
+		goto dealloc;
+	}
+	if (!(cardp->tx_urb = usb_alloc_urb(0, GFP_KERNEL))) {
+		lbs_deb_usbd(&udev->dev, "Tx URB allocation failed\n");
+		goto dealloc;
+	}
+	cardp->ep_out_buf = kmalloc(MRVDRV_ETH_TX_PACKET_BUFFER_SIZE, GFP_KERNEL);
+	if (!cardp->ep_out_buf) {
+		lbs_deb_usbd(&udev->dev, "Could not allocate buffer\n");
+		goto dealloc;
+	}
 
 	/* Upload firmware */
-	cardp->rinfo.cardp = cardp;
 	if (if_usb_prog_firmware(cardp)) {
-		lbs_deb_usbd(&udev->dev, "FW upload failed");
+		lbs_deb_usbd(&udev->dev, "FW upload failed\n");
 		goto err_prog_firmware;
 	}
 
@@ -240,7 +241,7 @@ static int if_usb_probe(struct usb_interface *intf,
 	if (lbs_start_card(priv))
 		goto err_start_card;
 
-	if_usb_set_boot2_ver(priv);
+	if_usb_setup_firmware(priv);
 
 	usb_get_dev(udev);
 	usb_set_intfdata(intf, cardp);
@@ -265,25 +266,19 @@ error:
  */
 static void if_usb_disconnect(struct usb_interface *intf)
 {
-	struct usb_card_rec *cardp = usb_get_intfdata(intf);
+	struct if_usb_card *cardp = usb_get_intfdata(intf);
 	struct lbs_private *priv = (struct lbs_private *) cardp->priv;
 
 	lbs_deb_enter(LBS_DEB_MAIN);
 
-	/* Update Surprise removed to TRUE */
 	cardp->surprise_removed = 1;
 
 	if (priv) {
-
 		priv->surpriseremoved = 1;
 		lbs_stop_card(priv);
 		lbs_remove_card(priv);
 	}
 
-	/* this is (apparently?) necessary for future usage of the device */
-	lbs_prepare_and_send_command(priv, CMD_802_11_RESET, CMD_ACT_HALT,
-			0, 0, NULL);
-
 	/* Unlink and free urb */
 	if_usb_free(cardp);
 
@@ -298,98 +293,75 @@ static void if_usb_disconnect(struct usb_interface *intf)
  *  @param priv		pointer to struct lbs_private
  *  @return 	   	0
  */
-static int if_usb_send_fw_pkt(struct usb_card_rec *cardp)
+static int if_usb_send_fw_pkt(struct if_usb_card *cardp)
 {
-	struct FWData *fwdata;
-	struct fwheader *fwheader;
-	u8 *firmware = cardp->fw->data;
-
-	fwdata = kmalloc(sizeof(struct FWData), GFP_ATOMIC);
-
-	if (!fwdata)
-		return -1;
-
-	fwheader = &fwdata->fwheader;
+	struct fwdata *fwdata = cardp->ep_out_buf;
+	uint8_t *firmware = cardp->fw->data;
 
+	/* If we got a CRC failure on the last block, back
+	   up and retry it */
 	if (!cardp->CRC_OK) {
 		cardp->totalbytes = cardp->fwlastblksent;
-		cardp->fwseqnum = cardp->lastseqnum - 1;
+		cardp->fwseqnum--;
 	}
 
-	/*
-	lbs_deb_usbd(&cardp->udev->dev, "totalbytes = %d\n",
-		    cardp->totalbytes);
-	*/
+	lbs_deb_usb2(&cardp->udev->dev, "totalbytes = %d\n",
+		     cardp->totalbytes);
 
-	memcpy(fwheader, &firmware[cardp->totalbytes],
+	/* struct fwdata (which we sent to the card) has an
+	   extra __le32 field in between the header and the data,
+	   which is not in the struct fwheader in the actual
+	   firmware binary. Insert the seqnum in the middle... */
+	memcpy(&fwdata->hdr, &firmware[cardp->totalbytes],
 	       sizeof(struct fwheader));
 
 	cardp->fwlastblksent = cardp->totalbytes;
 	cardp->totalbytes += sizeof(struct fwheader);
 
-	/* lbs_deb_usbd(&cardp->udev->dev,"Copy Data\n"); */
 	memcpy(fwdata->data, &firmware[cardp->totalbytes],
-	       le32_to_cpu(fwdata->fwheader.datalength));
+	       le32_to_cpu(fwdata->hdr.datalength));
 
-	/*
-	lbs_deb_usbd(&cardp->udev->dev,
-		    "Data length = %d\n", le32_to_cpu(fwdata->fwheader.datalength));
-	*/
+	lbs_deb_usb2(&cardp->udev->dev, "Data length = %d\n",
+		     le32_to_cpu(fwdata->hdr.datalength));
 
-	cardp->fwseqnum = cardp->fwseqnum + 1;
+	fwdata->seqnum = cpu_to_le32(++cardp->fwseqnum);
+	cardp->totalbytes += le32_to_cpu(fwdata->hdr.datalength);
 
-	fwdata->seqnum = cpu_to_le32(cardp->fwseqnum);
-	cardp->lastseqnum = cardp->fwseqnum;
-	cardp->totalbytes += le32_to_cpu(fwdata->fwheader.datalength);
+	usb_tx_block(cardp, cardp->ep_out_buf, sizeof(struct fwdata) +
+		     le32_to_cpu(fwdata->hdr.datalength));
+
+	if (fwdata->hdr.dnldcmd == cpu_to_le32(FW_HAS_DATA_TO_RECV)) {
+		lbs_deb_usb2(&cardp->udev->dev, "There are data to follow\n");
+		lbs_deb_usb2(&cardp->udev->dev, "seqnum = %d totalbytes = %d\n",
+			     cardp->fwseqnum, cardp->totalbytes);
+	} else if (fwdata->hdr.dnldcmd == cpu_to_le32(FW_HAS_LAST_BLOCK)) {
+		lbs_deb_usb2(&cardp->udev->dev, "Host has finished FW downloading\n");
+		lbs_deb_usb2(&cardp->udev->dev, "Donwloading FW JUMP BLOCK\n");
 
-	if (fwheader->dnldcmd == cpu_to_le32(FW_HAS_DATA_TO_RECV)) {
-		/*
-		lbs_deb_usbd(&cardp->udev->dev, "There are data to follow\n");
-		lbs_deb_usbd(&cardp->udev->dev,
-			    "seqnum = %d totalbytes = %d\n", cardp->fwseqnum,
-			    cardp->totalbytes);
-		*/
-		memcpy(cardp->bulk_out_buffer, fwheader, FW_DATA_XMIT_SIZE);
-		usb_tx_block(cardp, cardp->bulk_out_buffer, FW_DATA_XMIT_SIZE);
-
-	} else if (fwdata->fwheader.dnldcmd == cpu_to_le32(FW_HAS_LAST_BLOCK)) {
-		/*
-		lbs_deb_usbd(&cardp->udev->dev,
-			    "Host has finished FW downloading\n");
-		lbs_deb_usbd(&cardp->udev->dev,
-			    "Donwloading FW JUMP BLOCK\n");
-		*/
-		memcpy(cardp->bulk_out_buffer, fwheader, FW_DATA_XMIT_SIZE);
-		usb_tx_block(cardp, cardp->bulk_out_buffer, FW_DATA_XMIT_SIZE);
 		cardp->fwfinalblk = 1;
 	}
 
-	/*
-	lbs_deb_usbd(&cardp->udev->dev,
-		    "The firmware download is done size is %d\n",
-		    cardp->totalbytes);
-	*/
-
-	kfree(fwdata);
+	lbs_deb_usb2(&cardp->udev->dev, "Firmware download done; size %d\n",
+		     cardp->totalbytes);
 
 	return 0;
 }
 
-static int if_usb_reset_device(struct usb_card_rec *cardp)
+static int if_usb_reset_device(struct if_usb_card *cardp)
 {
-	struct cmd_ds_command *cmd = (void *)&cardp->bulk_out_buffer[4];
+	struct cmd_ds_command *cmd = cardp->ep_out_buf + 4;
 	int ret;
 
 	lbs_deb_enter(LBS_DEB_USB);
 
-	*(__le32 *)cardp->bulk_out_buffer = cpu_to_le32(CMD_TYPE_REQUEST);
+	*(__le32 *)cardp->ep_out_buf = cpu_to_le32(CMD_TYPE_REQUEST);
 
 	cmd->command = cpu_to_le16(CMD_802_11_RESET);
 	cmd->size = cpu_to_le16(sizeof(struct cmd_ds_802_11_reset) + S_DS_GEN);
 	cmd->result = cpu_to_le16(0);
 	cmd->seqnum = cpu_to_le16(0x5a5a);
 	cmd->params.reset.action = cpu_to_le16(CMD_ACT_HALT);
-	usb_tx_block(cardp, cardp->bulk_out_buffer, 4 + S_DS_GEN + sizeof(struct cmd_ds_802_11_reset));
+	usb_tx_block(cardp, cardp->ep_out_buf, 4 + S_DS_GEN + sizeof(struct cmd_ds_802_11_reset));
 
 	msleep(100);
 	ret = usb_reset_device(cardp->udev);
@@ -407,7 +379,7 @@ static int if_usb_reset_device(struct usb_card_rec *cardp)
  *  @param nb		data length
  *  @return 	   	0 or -1
  */
-static int usb_tx_block(struct usb_card_rec *cardp, u8 * payload, u16 nb)
+static int usb_tx_block(struct if_usb_card *cardp, uint8_t *payload, uint16_t nb)
 {
 	int ret = -1;
 
@@ -419,17 +391,16 @@ static int usb_tx_block(struct usb_card_rec *cardp, u8 * payload, u16 nb)
 
 	usb_fill_bulk_urb(cardp->tx_urb, cardp->udev,
 			  usb_sndbulkpipe(cardp->udev,
-					  cardp->bulk_out_endpointAddr),
+					  cardp->ep_out),
 			  payload, nb, if_usb_write_bulk_callback, cardp);
 
 	cardp->tx_urb->transfer_flags |= URB_ZERO_PACKET;
 
 	if ((ret = usb_submit_urb(cardp->tx_urb, GFP_ATOMIC))) {
-		/*  transfer failed */
 		lbs_deb_usbd(&cardp->udev->dev, "usb_submit_urb failed: %d\n", ret);
 		ret = -1;
 	} else {
-		/* lbs_deb_usbd(&cardp->udev->dev, "usb_submit_urb success\n"); */
+		lbs_deb_usb2(&cardp->udev->dev, "usb_submit_urb success\n");
 		ret = 0;
 	}
 
@@ -437,11 +408,10 @@ tx_ret:
 	return ret;
 }
 
-static int __if_usb_submit_rx_urb(struct usb_card_rec *cardp,
+static int __if_usb_submit_rx_urb(struct if_usb_card *cardp,
 				  void (*callbackfn)(struct urb *urb))
 {
 	struct sk_buff *skb;
-	struct read_cb_info *rinfo = &cardp->rinfo;
 	int ret = -1;
 
 	if (!(skb = dev_alloc_skb(MRVDRV_ETH_RX_PACKET_BUFFER_SIZE))) {
@@ -449,27 +419,25 @@ static int __if_usb_submit_rx_urb(struct usb_card_rec *cardp,
 		goto rx_ret;
 	}
 
-	rinfo->skb = skb;
+	cardp->rx_skb = skb;
 
 	/* Fill the receive configuration URB and initialise the Rx call back */
 	usb_fill_bulk_urb(cardp->rx_urb, cardp->udev,
-			  usb_rcvbulkpipe(cardp->udev,
-					  cardp->bulk_in_endpointAddr),
+			  usb_rcvbulkpipe(cardp->udev, cardp->ep_in),
 			  (void *) (skb->tail + (size_t) IPFIELD_ALIGN_OFFSET),
 			  MRVDRV_ETH_RX_PACKET_BUFFER_SIZE, callbackfn,
-			  rinfo);
+			  cardp);
 
 	cardp->rx_urb->transfer_flags |= URB_ZERO_PACKET;
 
-	/* lbs_deb_usbd(&cardp->udev->dev, "Pointer for rx_urb %p\n", cardp->rx_urb); */
+	lbs_deb_usb2(&cardp->udev->dev, "Pointer for rx_urb %p\n", cardp->rx_urb);
 	if ((ret = usb_submit_urb(cardp->rx_urb, GFP_ATOMIC))) {
-		/* handle failure conditions */
 		lbs_deb_usbd(&cardp->udev->dev, "Submit Rx URB failed: %d\n", ret);
 		kfree_skb(skb);
-		rinfo->skb = NULL;
+		cardp->rx_skb = NULL;
 		ret = -1;
 	} else {
-		/* lbs_deb_usbd(&cardp->udev->dev, "Submit Rx URB success\n"); */
+		lbs_deb_usb2(&cardp->udev->dev, "Submit Rx URB success\n");
 		ret = 0;
 	}
 
@@ -477,27 +445,26 @@ rx_ret:
 	return ret;
 }
 
-static int if_usb_submit_rx_urb_fwload(struct usb_card_rec *cardp)
+static int if_usb_submit_rx_urb_fwload(struct if_usb_card *cardp)
 {
 	return __if_usb_submit_rx_urb(cardp, &if_usb_receive_fwload);
 }
 
-static int if_usb_submit_rx_urb(struct usb_card_rec *cardp)
+static int if_usb_submit_rx_urb(struct if_usb_card *cardp)
 {
 	return __if_usb_submit_rx_urb(cardp, &if_usb_receive);
 }
 
 static void if_usb_receive_fwload(struct urb *urb)
 {
-	struct read_cb_info *rinfo = (struct read_cb_info *)urb->context;
-	struct sk_buff *skb = rinfo->skb;
-	struct usb_card_rec *cardp = (struct usb_card_rec *)rinfo->cardp;
+	struct if_usb_card *cardp = urb->context;
+	struct sk_buff *skb = cardp->rx_skb;
 	struct fwsyncheader *syncfwheader;
-	struct bootcmdrespStr bootcmdresp;
+	struct bootcmdresp bootcmdresp;
 
 	if (urb->status) {
 		lbs_deb_usbd(&cardp->udev->dev,
-			    "URB status is failed during fw load\n");
+			     "URB status is failed during fw load\n");
 		kfree_skb(skb);
 		return;
 	}
@@ -510,8 +477,8 @@ static void if_usb_receive_fwload(struct urb *urb)
 			lbs_pr_info("Firmware ready event received\n");
 			wake_up(&cardp->fw_wq);
 		} else {
-			lbs_deb_usb("Waiting for confirmation; got %x %x\n", le32_to_cpu(tmp[0]),
-				    le32_to_cpu(tmp[1]));
+			lbs_deb_usb("Waiting for confirmation; got %x %x\n",
+				    le32_to_cpu(tmp[0]), le32_to_cpu(tmp[1]));
 			if_usb_submit_rx_urb_fwload(cardp);
 		}
 		kfree_skb(skb);
@@ -520,37 +487,36 @@ static void if_usb_receive_fwload(struct urb *urb)
 	if (cardp->bootcmdresp <= 0) {
 		memcpy (&bootcmdresp, skb->data + IPFIELD_ALIGN_OFFSET,
 			sizeof(bootcmdresp));
+
 		if (le16_to_cpu(cardp->udev->descriptor.bcdDevice) < 0x3106) {
 			kfree_skb(skb);
 			if_usb_submit_rx_urb_fwload(cardp);
 			cardp->bootcmdresp = 1;
 			lbs_deb_usbd(&cardp->udev->dev,
-				    "Received valid boot command response\n");
+				     "Received valid boot command response\n");
 			return;
 		}
-		if (bootcmdresp.u32magicnumber != cpu_to_le32(BOOT_CMD_MAGIC_NUMBER)) {
-			if (bootcmdresp.u32magicnumber == cpu_to_le32(CMD_TYPE_REQUEST) ||
-			    bootcmdresp.u32magicnumber == cpu_to_le32(CMD_TYPE_DATA) ||
-			    bootcmdresp.u32magicnumber == cpu_to_le32(CMD_TYPE_INDICATION)) {
+		if (bootcmdresp.magic != cpu_to_le32(BOOT_CMD_MAGIC_NUMBER)) {
+			if (bootcmdresp.magic == cpu_to_le32(CMD_TYPE_REQUEST) ||
+			    bootcmdresp.magic == cpu_to_le32(CMD_TYPE_DATA) ||
+			    bootcmdresp.magic == cpu_to_le32(CMD_TYPE_INDICATION)) {
 				if (!cardp->bootcmdresp)
 					lbs_pr_info("Firmware already seems alive; resetting\n");
 				cardp->bootcmdresp = -1;
 			} else {
 				lbs_pr_info("boot cmd response wrong magic number (0x%x)\n",
-					    le32_to_cpu(bootcmdresp.u32magicnumber));
+					    le32_to_cpu(bootcmdresp.magic));
 			}
-		} else if (bootcmdresp.u8cmd_tag != BOOT_CMD_FW_BY_USB) {
-			lbs_pr_info(
-				"boot cmd response cmd_tag error (%d)\n",
-				bootcmdresp.u8cmd_tag);
-		} else if (bootcmdresp.u8result != BOOT_CMD_RESP_OK) {
-			lbs_pr_info(
-				"boot cmd response result error (%d)\n",
-				bootcmdresp.u8result);
+		} else if (bootcmdresp.cmd != BOOT_CMD_FW_BY_USB) {
+			lbs_pr_info("boot cmd response cmd_tag error (%d)\n",
+				    bootcmdresp.cmd);
+		} else if (bootcmdresp.result != BOOT_CMD_RESP_OK) {
+			lbs_pr_info("boot cmd response result error (%d)\n",
+				    bootcmdresp.result);
 		} else {
 			cardp->bootcmdresp = 1;
 			lbs_deb_usbd(&cardp->udev->dev,
-				    "Received valid boot command response\n");
+				     "Received valid boot command response\n");
 		}
 		kfree_skb(skb);
 		if_usb_submit_rx_urb_fwload(cardp);
@@ -565,20 +531,15 @@ static void if_usb_receive_fwload(struct urb *urb)
 	}
 
 	memcpy(syncfwheader, skb->data + IPFIELD_ALIGN_OFFSET,
-			sizeof(struct fwsyncheader));
+	       sizeof(struct fwsyncheader));
 
 	if (!syncfwheader->cmd) {
-		/*
-		lbs_deb_usbd(&cardp->udev->dev,
-			    "FW received Blk with correct CRC\n");
-		lbs_deb_usbd(&cardp->udev->dev,
-			    "FW received Blk seqnum = %d\n",
-		       syncfwheader->seqnum);
-		*/
+		lbs_deb_usb2(&cardp->udev->dev, "FW received Blk with correct CRC\n");
+		lbs_deb_usb2(&cardp->udev->dev, "FW received Blk seqnum = %d\n",
+			     le32_to_cpu(syncfwheader->seqnum));
 		cardp->CRC_OK = 1;
 	} else {
-		lbs_deb_usbd(&cardp->udev->dev,
-			    "FW received Blk with CRC error\n");
+		lbs_deb_usbd(&cardp->udev->dev, "FW received Blk with CRC error\n");
 		cardp->CRC_OK = 0;
 	}
 
@@ -605,13 +566,12 @@ static void if_usb_receive_fwload(struct urb *urb)
 #define MRVDRV_MIN_PKT_LEN	30
 
 static inline void process_cmdtypedata(int recvlength, struct sk_buff *skb,
-				       struct usb_card_rec *cardp,
+				       struct if_usb_card *cardp,
 				       struct lbs_private *priv)
 {
-	if (recvlength > MRVDRV_ETH_RX_PACKET_BUFFER_SIZE +
-	    MESSAGE_HEADER_LEN || recvlength < MRVDRV_MIN_PKT_LEN) {
-		lbs_deb_usbd(&cardp->udev->dev,
-			    "Packet length is Invalid\n");
+	if (recvlength > MRVDRV_ETH_RX_PACKET_BUFFER_SIZE + MESSAGE_HEADER_LEN
+	    || recvlength < MRVDRV_MIN_PKT_LEN) {
+		lbs_deb_usbd(&cardp->udev->dev, "Packet length is Invalid\n");
 		kfree_skb(skb);
 		return;
 	}
@@ -619,19 +579,19 @@ static inline void process_cmdtypedata(int recvlength, struct sk_buff *skb,
 	skb_reserve(skb, IPFIELD_ALIGN_OFFSET);
 	skb_put(skb, recvlength);
 	skb_pull(skb, MESSAGE_HEADER_LEN);
+
 	lbs_process_rxed_packet(priv, skb);
 	priv->upld_len = (recvlength - MESSAGE_HEADER_LEN);
 }
 
-static inline void process_cmdrequest(int recvlength, u8 *recvbuff,
+static inline void process_cmdrequest(int recvlength, uint8_t *recvbuff,
 				      struct sk_buff *skb,
-				      struct usb_card_rec *cardp,
+				      struct if_usb_card *cardp,
 				      struct lbs_private *priv)
 {
-	u8 *cmdbuf;
 	if (recvlength > LBS_CMD_BUFFER_SIZE) {
 		lbs_deb_usbd(&cardp->udev->dev,
-			    "The receive buffer is too large\n");
+			     "The receive buffer is too large\n");
 		kfree_skb(skb);
 		return;
 	}
@@ -640,18 +600,9 @@ static inline void process_cmdrequest(int recvlength, u8 *recvbuff,
 		BUG();
 
 	spin_lock(&priv->driver_lock);
-	/* take care of cur_cmd = NULL case by reading the
-	 * data to clear the interrupt */
-	if (!priv->cur_cmd) {
-		cmdbuf = priv->upld_buf;
-		priv->hisregcpy &= ~MRVDRV_CMD_UPLD_RDY;
-	} else
-		cmdbuf = (u8 *) priv->cur_cmd->cmdbuf;
-
 	cardp->usb_int_cause |= MRVDRV_CMD_UPLD_RDY;
 	priv->upld_len = (recvlength - MESSAGE_HEADER_LEN);
-	memcpy(cmdbuf, recvbuff + MESSAGE_HEADER_LEN,
-	       priv->upld_len);
+	memcpy(priv->upld_buf, recvbuff + MESSAGE_HEADER_LEN, priv->upld_len);
 
 	kfree_skb(skb);
 	lbs_interrupt(priv);
@@ -659,8 +610,6 @@ static inline void process_cmdrequest(int recvlength, u8 *recvbuff,
 
 	lbs_deb_usbd(&cardp->udev->dev,
 		    "Wake up main thread to handle cmd response\n");
-
-	return;
 }
 
 /**
@@ -672,30 +621,26 @@ static inline void process_cmdrequest(int recvlength, u8 *recvbuff,
  */
 static void if_usb_receive(struct urb *urb)
 {
-	struct read_cb_info *rinfo = (struct read_cb_info *)urb->context;
-	struct sk_buff *skb = rinfo->skb;
-	struct usb_card_rec *cardp = (struct usb_card_rec *) rinfo->cardp;
+	struct if_usb_card *cardp = urb->context;
+	struct sk_buff *skb = cardp->rx_skb;
 	struct lbs_private *priv = cardp->priv;
-
 	int recvlength = urb->actual_length;
-	u8 *recvbuff = NULL;
-	u32 recvtype = 0;
+	uint8_t *recvbuff = NULL;
+	uint32_t recvtype = 0;
+	__le32 *pkt = (__le32 *)(skb->data + IPFIELD_ALIGN_OFFSET);
 
 	lbs_deb_enter(LBS_DEB_USB);
 
 	if (recvlength) {
-		__le32 tmp;
-
 		if (urb->status) {
-			lbs_deb_usbd(&cardp->udev->dev,
-				    "URB status is failed\n");
+			lbs_deb_usbd(&cardp->udev->dev, "RX URB failed: %d\n",
+				     urb->status);
 			kfree_skb(skb);
 			goto setup_for_next;
 		}
 
 		recvbuff = skb->data + IPFIELD_ALIGN_OFFSET;
-		memcpy(&tmp, recvbuff, sizeof(u32));
-		recvtype = le32_to_cpu(tmp);
+		recvtype = le32_to_cpu(pkt[0]);
 		lbs_deb_usbd(&cardp->udev->dev,
 			    "Recv length = 0x%x, Recv type = 0x%X\n",
 			    recvlength, recvtype);
@@ -716,9 +661,13 @@ static void if_usb_receive(struct urb *urb)
 	case CMD_TYPE_INDICATION:
 		/* Event cause handling */
 		spin_lock(&priv->driver_lock);
-		cardp->usb_event_cause = le32_to_cpu(*(__le32 *) (recvbuff + MESSAGE_HEADER_LEN));
+
+		cardp->usb_event_cause = le32_to_cpu(pkt[1]);
+
 		lbs_deb_usbd(&cardp->udev->dev,"**EVENT** 0x%X\n",
-			    cardp->usb_event_cause);
+			     cardp->usb_event_cause);
+
+		/* Icky undocumented magic special case */
 		if (cardp->usb_event_cause & 0xffff0000) {
 			lbs_send_tx_feedback(priv);
 			spin_unlock(&priv->driver_lock);
@@ -732,7 +681,7 @@ static void if_usb_receive(struct urb *urb)
 		goto rx_exit;
 	default:
 		lbs_deb_usbd(&cardp->udev->dev, "Unknown command type 0x%X\n",
-		             recvtype);
+			     recvtype);
 		kfree_skb(skb);
 		break;
 	}
@@ -751,55 +700,48 @@ rx_exit:
  *  @param len		number of bytes
  *  @return 	   	0 or -1
  */
-static int if_usb_host_to_card(struct lbs_private *priv,
-	u8 type,
-	u8 *payload,
-	u16 nb)
+static int if_usb_host_to_card(struct lbs_private *priv, uint8_t type,
+			       uint8_t *payload, uint16_t nb)
 {
-	struct usb_card_rec *cardp = (struct usb_card_rec *)priv->card;
+	struct if_usb_card *cardp = priv->card;
 
 	lbs_deb_usbd(&cardp->udev->dev,"*** type = %u\n", type);
 	lbs_deb_usbd(&cardp->udev->dev,"size after = %d\n", nb);
 
 	if (type == MVMS_CMD) {
-		__le32 tmp = cpu_to_le32(CMD_TYPE_REQUEST);
+		*(__le32 *)cardp->ep_out_buf = cpu_to_le32(CMD_TYPE_REQUEST);
 		priv->dnld_sent = DNLD_CMD_SENT;
-		memcpy(cardp->bulk_out_buffer, (u8 *) & tmp,
-		       MESSAGE_HEADER_LEN);
-
 	} else {
-		__le32 tmp = cpu_to_le32(CMD_TYPE_DATA);
+		*(__le32 *)cardp->ep_out_buf = cpu_to_le32(CMD_TYPE_DATA);
 		priv->dnld_sent = DNLD_DATA_SENT;
-		memcpy(cardp->bulk_out_buffer, (u8 *) & tmp,
-		       MESSAGE_HEADER_LEN);
 	}
 
-	memcpy((cardp->bulk_out_buffer + MESSAGE_HEADER_LEN), payload, nb);
+	memcpy((cardp->ep_out_buf + MESSAGE_HEADER_LEN), payload, nb);
 
-	return usb_tx_block(cardp, cardp->bulk_out_buffer,
-	                    nb + MESSAGE_HEADER_LEN);
+	return usb_tx_block(cardp, cardp->ep_out_buf, nb + MESSAGE_HEADER_LEN);
 }
 
 /* called with priv->driver_lock held */
-static int if_usb_get_int_status(struct lbs_private *priv, u8 *ireg)
+static int if_usb_get_int_status(struct lbs_private *priv, uint8_t *ireg)
 {
-	struct usb_card_rec *cardp = priv->card;
+	struct if_usb_card *cardp = priv->card;
 
 	*ireg = cardp->usb_int_cause;
 	cardp->usb_int_cause = 0;
 
-	lbs_deb_usbd(&cardp->udev->dev,"Int cause is 0x%X\n", *ireg);
+	lbs_deb_usbd(&cardp->udev->dev, "Int cause is 0x%X\n", *ireg);
 
 	return 0;
 }
 
 static int if_usb_read_event_cause(struct lbs_private *priv)
 {
-	struct usb_card_rec *cardp = priv->card;
+	struct if_usb_card *cardp = priv->card;
 
 	priv->eventcause = cardp->usb_event_cause;
 	/* Re-submit rx urb here to avoid event lost issue */
 	if_usb_submit_rx_urb(cardp);
+
 	return 0;
 }
 
@@ -809,20 +751,17 @@ static int if_usb_read_event_cause(struct lbs_private *priv)
  *                  2:Boot from FW in EEPROM
  *  @return 	   	0
  */
-static int if_usb_issue_boot_command(struct usb_card_rec *cardp, int ivalue)
+static int if_usb_issue_boot_command(struct if_usb_card *cardp, int ivalue)
 {
-	struct bootcmdstr sbootcmd;
-	int i;
+	struct bootcmd *bootcmd = cardp->ep_out_buf;
 
 	/* Prepare command */
-	sbootcmd.u32magicnumber = cpu_to_le32(BOOT_CMD_MAGIC_NUMBER);
-	sbootcmd.u8cmd_tag = ivalue;
-	for (i=0; i<11; i++)
-		sbootcmd.au8dumy[i]=0x00;
-	memcpy(cardp->bulk_out_buffer, &sbootcmd, sizeof(struct bootcmdstr));
+	bootcmd->magic = cpu_to_le32(BOOT_CMD_MAGIC_NUMBER);
+	bootcmd->cmd = ivalue;
+	memset(bootcmd->pad, 0, sizeof(bootcmd->pad));
 
 	/* Issue command */
-	usb_tx_block(cardp, cardp->bulk_out_buffer, sizeof(struct bootcmdstr));
+	usb_tx_block(cardp, cardp->ep_out_buf, sizeof(*bootcmd));
 
 	return 0;
 }
@@ -835,10 +774,10 @@ static int if_usb_issue_boot_command(struct usb_card_rec *cardp, int ivalue)
  *         len               image length
  *  @return     0 or -1
  */
-static int check_fwfile_format(u8 *data, u32 totlen)
+static int check_fwfile_format(uint8_t *data, uint32_t totlen)
 {
-	u32 bincmd, exit;
-	u32 blksize, offset, len;
+	uint32_t bincmd, exit;
+	uint32_t blksize, offset, len;
 	int ret;
 
 	ret = 1;
@@ -876,7 +815,7 @@ static int check_fwfile_format(u8 *data, u32 totlen)
 }
 
 
-static int if_usb_prog_firmware(struct usb_card_rec *cardp)
+static int if_usb_prog_firmware(struct if_usb_card *cardp)
 {
 	int i = 0;
 	static int reset_count = 10;
@@ -937,7 +876,7 @@ restart:
 
 	/* ... and wait for the process to complete */
 	wait_event_interruptible(cardp->fw_wq, cardp->surprise_removed || cardp->fwdnldover);
-	
+
 	del_timer_sync(&cardp->fw_timeout);
 	usb_kill_urb(cardp->rx_urb);
 
@@ -953,11 +892,11 @@ restart:
 		goto release_fw;
 	}
 
-release_fw:
+ release_fw:
 	release_firmware(cardp->fw);
 	cardp->fw = NULL;
 
-done:
+ done:
 	lbs_deb_leave_args(LBS_DEB_USB, "ret %d", ret);
 	return ret;
 }
@@ -966,36 +905,38 @@ done:
 #ifdef CONFIG_PM
 static int if_usb_suspend(struct usb_interface *intf, pm_message_t message)
 {
-	struct usb_card_rec *cardp = usb_get_intfdata(intf);
+	struct if_usb_card *cardp = usb_get_intfdata(intf);
 	struct lbs_private *priv = cardp->priv;
+	int ret;
 
 	lbs_deb_enter(LBS_DEB_USB);
 
 	if (priv->psstate != PS_STATE_FULL_POWER)
 		return -1;
 
-	netif_device_detach(priv->dev);
-	netif_device_detach(priv->mesh_dev);
+	ret = lbs_suspend(priv);
+	if (ret)
+		goto out;
 
 	/* Unlink tx & rx urb */
 	usb_kill_urb(cardp->tx_urb);
 	usb_kill_urb(cardp->rx_urb);
 
+ out:
 	lbs_deb_leave(LBS_DEB_USB);
-	return 0;
+	return ret;
 }
 
 static int if_usb_resume(struct usb_interface *intf)
 {
-	struct usb_card_rec *cardp = usb_get_intfdata(intf);
+	struct if_usb_card *cardp = usb_get_intfdata(intf);
 	struct lbs_private *priv = cardp->priv;
 
 	lbs_deb_enter(LBS_DEB_USB);
 
 	if_usb_submit_rx_urb(cardp);
 
-	netif_device_attach(priv->dev);
-	netif_device_attach(priv->mesh_dev);
+	lbs_resume(priv);
 
 	lbs_deb_leave(LBS_DEB_USB);
 	return 0;
@@ -1039,5 +980,5 @@ module_init(if_usb_init_module);
 module_exit(if_usb_exit_module);
 
 MODULE_DESCRIPTION("8388 USB WLAN Driver");
-MODULE_AUTHOR("Marvell International Ltd.");
+MODULE_AUTHOR("Marvell International Ltd. and Red Hat, Inc.");
 MODULE_LICENSE("GPL");
diff --git a/package/libertas/src/if_usb.h b/package/libertas/src/if_usb.h
index 668410f820..7706691123 100644
--- a/package/libertas/src/if_usb.h
+++ b/package/libertas/src/if_usb.h
@@ -9,72 +9,67 @@ struct lbs_private;
 /**
   * This file contains definition for USB interface.
   */
-#define CMD_TYPE_REQUEST                0xF00DFACE
-#define CMD_TYPE_DATA                   0xBEADC0DE
-#define CMD_TYPE_INDICATION             0xBEEFFACE
+#define CMD_TYPE_REQUEST		0xF00DFACE
+#define CMD_TYPE_DATA			0xBEADC0DE
+#define CMD_TYPE_INDICATION		0xBEEFFACE
 
-#define IPFIELD_ALIGN_OFFSET	2
+#define IPFIELD_ALIGN_OFFSET		2
 
-#define BOOT_CMD_FW_BY_USB     0x01
-#define BOOT_CMD_FW_IN_EEPROM  0x02
-#define BOOT_CMD_UPDATE_BOOT2  0x03
-#define BOOT_CMD_UPDATE_FW     0x04
-#define BOOT_CMD_MAGIC_NUMBER  0x4C56524D   /* M=>0x4D,R=>0x52,V=>0x56,L=>0x4C */
+#define BOOT_CMD_FW_BY_USB		0x01
+#define BOOT_CMD_FW_IN_EEPROM		0x02
+#define BOOT_CMD_UPDATE_BOOT2		0x03
+#define BOOT_CMD_UPDATE_FW		0x04
+#define BOOT_CMD_MAGIC_NUMBER		0x4C56524D   /* LVRM */
 
-struct bootcmdstr
+struct bootcmd
 {
-	__le32 u32magicnumber;
-	u8  u8cmd_tag;
-	u8  au8dumy[11];
+	__le32	magic;
+	uint8_t	cmd;
+	uint8_t	pad[11];
 };
 
-#define BOOT_CMD_RESP_OK     0x0001
-#define BOOT_CMD_RESP_FAIL   0x0000
+#define BOOT_CMD_RESP_OK		0x0001
+#define BOOT_CMD_RESP_FAIL		0x0000
 
-struct bootcmdrespStr
+struct bootcmdresp
 {
-	__le32 u32magicnumber;
-	u8  u8cmd_tag;
-	u8  u8result;
-	u8  au8dumy[2];
-};
-
-/* read callback private data */
-struct read_cb_info {
-        struct usb_card_rec *cardp;
-        struct sk_buff *skb;
+	__le32	magic;
+	uint8_t	cmd;
+	uint8_t	result;
+	uint8_t	pad[2];
 };
 
 /** USB card description structure*/
-struct usb_card_rec {
+struct if_usb_card {
 	struct usb_device *udev;
 	struct urb *rx_urb, *tx_urb;
 	struct lbs_private *priv;
-	struct read_cb_info rinfo;
 
-	int bulk_in_size;
-	u8 bulk_in_endpointAddr;
+	struct sk_buff *rx_skb;
+	uint32_t usb_event_cause;
+	uint8_t usb_int_cause;
+
+	uint8_t ep_in;
+	uint8_t ep_out;
 
-	u8 *bulk_out_buffer;
-	int bulk_out_size;
-	u8 bulk_out_endpointAddr;
+	int8_t bootcmdresp;
+
+	int ep_in_size;
+
+	void *ep_out_buf;
+	int ep_out_size;
 
 	const struct firmware *fw;
 	struct timer_list fw_timeout;
 	wait_queue_head_t fw_wq;
-	u8 CRC_OK;
-	u32 fwseqnum;
-	u32 lastseqnum;
-	u32 totalbytes;
-	u32 fwlastblksent;
-	u8 fwdnldover;
-	u8 fwfinalblk;
-	u8 surprise_removed;
-
-	u32 usb_event_cause;
-	u8 usb_int_cause;
-
-	s8 bootcmdresp;
+	uint32_t fwseqnum;
+	uint32_t totalbytes;
+	uint32_t fwlastblksent;
+	uint8_t CRC_OK;
+	uint8_t fwdnldover;
+	uint8_t fwfinalblk;
+	uint8_t surprise_removed;
+
 };
 
 /** fwheader */
@@ -87,10 +82,10 @@ struct fwheader {
 
 #define FW_MAX_DATA_BLK_SIZE	600
 /** FWData */
-struct FWData {
-	struct fwheader fwheader;
+struct fwdata {
+	struct fwheader hdr;
 	__le32 seqnum;
-	u8 data[FW_MAX_DATA_BLK_SIZE];
+	uint8_t data[0];
 };
 
 /** fwsyncheader */
@@ -102,7 +97,5 @@ struct fwsyncheader {
 #define FW_HAS_DATA_TO_RECV		0x00000001
 #define FW_HAS_LAST_BLOCK		0x00000004
 
-#define FW_DATA_XMIT_SIZE \
-	sizeof(struct fwheader) + le32_to_cpu(fwdata->fwheader.datalength) + sizeof(u32)
 
 #endif
diff --git a/package/libertas/src/join.c b/package/libertas/src/join.c
index 14425d9a19..2d4508048b 100644
--- a/package/libertas/src/join.c
+++ b/package/libertas/src/join.c
@@ -781,8 +781,8 @@ int lbs_ret_80211_associate(struct lbs_private *priv,
 	priv->numSNRNF = 0;
 
 	netif_carrier_on(priv->dev);
-	netif_wake_queue(priv->dev);
-
+	if (!priv->tx_pending_len)
+		netif_wake_queue(priv->dev);
 
 	memcpy(wrqu.ap_addr.sa_data, priv->curbssparams.bssid, ETH_ALEN);
 	wrqu.ap_addr.sa_family = ARPHRD_ETHER;
@@ -865,7 +865,8 @@ int lbs_ret_80211_ad_hoc_start(struct lbs_private *priv,
 	priv->curbssparams.ssid_len = bss->ssid_len;
 
 	netif_carrier_on(priv->dev);
-	netif_wake_queue(priv->dev);
+	if (!priv->tx_pending_len)
+		netif_wake_queue(priv->dev);
 
 	memset(&wrqu, 0, sizeof(wrqu));
 	memcpy(wrqu.ap_addr.sa_data, priv->curbssparams.bssid, ETH_ALEN);
diff --git a/package/libertas/src/main.c b/package/libertas/src/main.c
index 406d874075..91b2f2398a 100644
--- a/package/libertas/src/main.c
+++ b/package/libertas/src/main.c
@@ -6,7 +6,6 @@
 
 #include <linux/moduleparam.h>
 #include <linux/delay.h>
-#include <linux/freezer.h>
 #include <linux/etherdevice.h>
 #include <linux/netdevice.h>
 #include <linux/if_arp.h>
@@ -256,7 +255,7 @@ static int lbs_add_rtap(struct lbs_private *priv);
 static void lbs_remove_rtap(struct lbs_private *priv);
 static int lbs_add_mesh(struct lbs_private *priv);
 static void lbs_remove_mesh(struct lbs_private *priv);
-  
+
 
 /**
  * Get function for sysfs attribute rtap
@@ -345,10 +344,10 @@ static ssize_t lbs_mesh_set(struct device *dev,
 	if (enable == !!priv->mesh_dev)
 		return count;
 
-	ret = lbs_mesh_config(priv, enable);
+	ret = lbs_mesh_config(priv, enable, priv->curbssparams.channel);
 	if (ret)
 		return ret;
-		
+
 	if (enable)
 		lbs_add_mesh(priv);
 	else
@@ -402,7 +401,7 @@ static int lbs_dev_open(struct net_device *dev)
 		netif_carrier_on(dev);
 	} else {
 		priv->infra_open = 1;
-		
+
 		if (priv->connect_status == LBS_CONNECTED)
 			netif_carrier_on(dev);
 		else
@@ -434,7 +433,7 @@ static int lbs_mesh_stop(struct net_device *dev)
 
 	netif_stop_queue(dev);
 	netif_carrier_off(dev);
-	
+
 	spin_unlock_irq(&priv->driver_lock);
 	return 0;
 }
@@ -454,7 +453,7 @@ static int lbs_eth_stop(struct net_device *dev)
 	priv->infra_open = 0;
 
 	netif_stop_queue(dev);
-	
+
 	spin_unlock_irq(&priv->driver_lock);
 	return 0;
 }
@@ -477,6 +476,13 @@ static void lbs_tx_timeout(struct net_device *dev)
 	   to kick it somehow? */
 	lbs_host_to_card_done(priv);
 
+	/* More often than not, this actually happens because the
+	   firmware has crapped itself -- rather than just a very
+	   busy medium. So send a harmless command, and if/when
+	   _that_ times out, we'll kick it in the head. */
+	lbs_prepare_and_send_command(priv, CMD_802_11_RSSI, 0,
+				     0, 0, NULL);
+
 	lbs_deb_leave(LBS_DEB_TX);
 }
 
@@ -489,19 +495,9 @@ void lbs_host_to_card_done(struct lbs_private *priv)
 	priv->dnld_sent = DNLD_RES_RECEIVED;
 
 	/* Wake main thread if commands are pending */
-	if (!priv->cur_cmd)
+	if (!priv->cur_cmd || priv->tx_pending_len > 0)
 		wake_up_interruptible(&priv->waitq);
 
-	/* Don't wake netif queues if we're in monitor mode and
-	   a TX packet is already pending, or if there are commands
-	   queued to be sent. */
-	if (!priv->currenttxskb && list_empty(&priv->cmdpendingq)) {
-		if (priv->dev && priv->connect_status == LBS_CONNECTED)
-			netif_wake_queue(priv->dev);
-
-		if (priv->mesh_dev && priv->mesh_connect_status == LBS_CONNECTED)
-			netif_wake_queue(priv->mesh_dev);
-	}
 	spin_unlock_irqrestore(&priv->driver_lock, flags);
 }
 EXPORT_SYMBOL_GPL(lbs_host_to_card_done);
@@ -663,8 +659,6 @@ static int lbs_thread(void *data)
 
 	init_waitqueue_entry(&wait, current);
 
-	set_freezable();
-
 	for (;;) {
 		int shouldsleep;
 
@@ -675,12 +669,16 @@ static int lbs_thread(void *data)
 		set_current_state(TASK_INTERRUPTIBLE);
 		spin_lock_irq(&priv->driver_lock);
 
-		if (priv->surpriseremoved)
+		if (kthread_should_stop())
 			shouldsleep = 0;	/* Bye */
+		else if (priv->surpriseremoved)
+			shouldsleep = 1;	/* We need to wait until we're _told_ to die */
 		else if (priv->psstate == PS_STATE_SLEEP)
 			shouldsleep = 1;	/* Sleep mode. Nothing we can do till it wakes */
 		else if (priv->intcounter)
 			shouldsleep = 0;	/* Interrupt pending. Deal with it now */
+		else if (priv->cmd_timed_out)
+			shouldsleep = 0;	/* Command timed out. Recover */
 		else if (!priv->fw_ready)
 			shouldsleep = 1;	/* Firmware not ready. We're waiting for it */
 		else if (priv->dnld_sent)
@@ -708,17 +706,19 @@ static int lbs_thread(void *data)
 
 		set_current_state(TASK_RUNNING);
 		remove_wait_queue(&priv->waitq, &wait);
-		try_to_freeze();
 
 		lbs_deb_thread("main-thread 333: intcounter=%d currenttxskb=%p dnld_sent=%d\n",
 			       priv->intcounter, priv->currenttxskb, priv->dnld_sent);
 
-		if (kthread_should_stop() || priv->surpriseremoved) {
-			lbs_deb_thread("main-thread: break from main thread: surpriseremoved=0x%x\n",
-				       priv->surpriseremoved);
+		if (kthread_should_stop()) {
+			lbs_deb_thread("main-thread: break from main thread\n");
 			break;
 		}
 
+		if (priv->surpriseremoved) {
+			lbs_deb_thread("adapter removed; waiting to die...\n");
+			continue;
+		}
 
 		spin_lock_irq(&priv->driver_lock);
 
@@ -749,6 +749,26 @@ static int lbs_thread(void *data)
 			spin_lock_irq(&priv->driver_lock);
 		}
 
+		if (priv->cmd_timed_out && priv->cur_cmd) {
+			struct cmd_ctrl_node *cmdnode = priv->cur_cmd;
+
+			if (++priv->nr_retries > 10) {
+				lbs_pr_info("Excessive timeouts submitting command %x\n",
+					    le16_to_cpu(cmdnode->cmdbuf->command));
+				lbs_complete_command(priv, cmdnode, -ETIMEDOUT);
+				priv->nr_retries = 0;
+			} else {
+				priv->cur_cmd = NULL;
+				lbs_pr_info("requeueing command %x due to timeout (#%d)\n",
+					    le16_to_cpu(cmdnode->cmdbuf->command), priv->nr_retries);
+
+				/* Stick it back at the _top_ of the pending queue
+				   for immediate resubmission */
+				list_add(&cmdnode->list, &priv->cmdpendingq);
+			}
+		}
+		priv->cmd_timed_out = 0;
+
 		/* Any Card Event */
 		if (priv->hisregcpy & MRVDRV_CARDEVENT) {
 			lbs_deb_thread("main-thread: Card Event Activity\n");
@@ -834,6 +854,58 @@ static int lbs_thread(void *data)
 	return 0;
 }
 
+static int lbs_suspend_callback(struct lbs_private *priv, unsigned long dummy,
+				struct cmd_header *cmd)
+{
+	lbs_deb_fw("HOST_SLEEP_ACTIVATE succeeded\n");
+
+	netif_device_detach(priv->dev);
+	if (priv->mesh_dev)
+		netif_device_detach(priv->mesh_dev);
+
+	priv->fw_ready = 0;
+	return 0;
+}
+
+
+int lbs_suspend(struct lbs_private *priv)
+{
+	struct cmd_header cmd;
+	int ret;
+
+	if (priv->wol_criteria == 0xffffffff) {
+		lbs_pr_info("Suspend attempt without configuring wake params!\n");
+		return -EINVAL;
+	}
+
+	memset(&cmd, 0, sizeof(cmd));
+
+	ret = __lbs_cmd(priv, CMD_802_11_HOST_SLEEP_ACTIVATE, &cmd,
+			sizeof(cmd), lbs_suspend_callback, 0);
+	if (ret)
+		lbs_pr_info("HOST_SLEEP_ACTIVATE failed: %d\n", ret);
+
+	return ret;
+}
+EXPORT_SYMBOL_GPL(lbs_suspend);
+
+int lbs_resume(struct lbs_private *priv)
+{
+	priv->fw_ready = 1;
+
+	/* Firmware doesn't seem to give us RX packets any more
+	   until we send it some command. Might as well update */
+	lbs_prepare_and_send_command(priv, CMD_802_11_RSSI, 0,
+				     0, 0, NULL);
+
+	netif_device_attach(priv->dev);
+	if (priv->mesh_dev)
+		netif_device_attach(priv->mesh_dev);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(lbs_resume);
+
 /**
  *  @brief This function downloads firmware image, gets
  *  HW spec from firmware and set basic parameters to
@@ -879,35 +951,21 @@ done:
 static void command_timer_fn(unsigned long data)
 {
 	struct lbs_private *priv = (struct lbs_private *)data;
-	struct cmd_ctrl_node *node;
 	unsigned long flags;
 
-	node = priv->cur_cmd;
-	if (node == NULL) {
-		lbs_deb_fw("ptempnode empty\n");
-		return;
-	}
+	spin_lock_irqsave(&priv->driver_lock, flags);
 
-	if (!node->cmdbuf) {
-		lbs_deb_fw("cmd is NULL\n");
-		return;
+	if (!priv->cur_cmd) {
+		lbs_pr_info("Command timer expired; no pending command\n");
+		goto out;
 	}
 
-	lbs_deb_fw("command_timer_fn fired, cmd %x\n", node->cmdbuf->command);
-
-	if (!priv->fw_ready)
-		return;
-
-	spin_lock_irqsave(&priv->driver_lock, flags);
-	priv->cur_cmd = NULL;
-	spin_unlock_irqrestore(&priv->driver_lock, flags);
-
-	lbs_deb_fw("re-sending same command because of timeout\n");
-	lbs_queue_cmd(priv, node, 0);
+	lbs_pr_info("Command %x timed out\n", le16_to_cpu(priv->cur_cmd->cmdbuf->command));
 
+	priv->cmd_timed_out = 1;
 	wake_up_interruptible(&priv->waitq);
-
-	return;
+ out:
+	spin_unlock_irqrestore(&priv->driver_lock, flags);
 }
 
 static int lbs_init_adapter(struct lbs_private *priv)
@@ -1055,6 +1113,9 @@ struct lbs_private *lbs_add_card(void *card, struct device *dmdev)
 	sprintf(priv->mesh_ssid, "mesh");
 	priv->mesh_ssid_len = 4;
 
+	priv->wol_criteria = 0xffffffff;
+	priv->wol_gpio = 0xff;
+
 	goto done;
 
 err_init_adapter:
@@ -1130,8 +1191,33 @@ int lbs_start_card(struct lbs_private *priv)
 	}
 	if (device_create_file(&dev->dev, &dev_attr_lbs_rtap))
 		lbs_pr_err("cannot register lbs_rtap attribute\n");
-	if (device_create_file(&dev->dev, &dev_attr_lbs_mesh))
-		lbs_pr_err("cannot register lbs_mesh attribute\n");
+
+	/* Enable mesh, if supported, and work out which TLV it uses.
+	   0x100 + 291 is an unofficial value used in 5.110.20.pXX
+	   0x100 + 37 is the official value used in 5.110.21.pXX
+	   but we check them in that order because 20.pXX doesn't
+	   give an error -- it just silently fails. */
+
+	/* 5.110.20.pXX firmware will fail the command if the channel
+	   doesn't match the existing channel. But only if the TLV
+	   is correct. If the channel is wrong, _BOTH_ versions will
+	   give an error to 0x100+291, and allow 0x100+37 to succeed.
+	   It's just that 5.110.20.pXX will not have done anything
+	   useful */
+
+	lbs_update_channel(priv);
+	priv->mesh_tlv = 0x100 + 291;
+	if (lbs_mesh_config(priv, 1, priv->curbssparams.channel)) {
+		priv->mesh_tlv = 0x100 + 37;
+		if (lbs_mesh_config(priv, 1, priv->curbssparams.channel))
+			priv->mesh_tlv = 0;
+	}
+	if (priv->mesh_tlv) {
+		lbs_add_mesh(priv);
+
+		if (device_create_file(&dev->dev, &dev_attr_lbs_mesh))
+			lbs_pr_err("cannot register lbs_mesh attribute\n");
+	}
 
 	lbs_debugfs_init_one(priv, dev);
 
@@ -1160,11 +1246,13 @@ int lbs_stop_card(struct lbs_private *priv)
 
 	lbs_debugfs_remove_one(priv);
 	device_remove_file(&dev->dev, &dev_attr_lbs_rtap);
-	device_remove_file(&dev->dev, &dev_attr_lbs_mesh);
+	if (priv->mesh_tlv)
+		device_remove_file(&dev->dev, &dev_attr_lbs_mesh);
 
 	/* Flush pending command nodes */
 	spin_lock_irqsave(&priv->driver_lock, flags);
 	list_for_each_entry(cmdnode, &priv->cmdpendingq, list) {
+		cmdnode->result = -ENOENT;
 		cmdnode->cmdwaitqwoken = 1;
 		wake_up_interruptible(&cmdnode->cmdwait_q);
 	}
@@ -1347,12 +1435,6 @@ void lbs_interrupt(struct lbs_private *priv)
 
 	lbs_deb_thread("lbs_interrupt: intcounter=%d\n", priv->intcounter);
 
-	if (!spin_trylock(&priv->driver_lock)) {
-		spin_unlock(&priv->driver_lock);
-		printk(KERN_CRIT "%s called without driver_lock held\n", __func__);
-		WARN_ON(1);
-	}
-
 	priv->intcounter++;
 
 	if (priv->psstate == PS_STATE_SLEEP)
diff --git a/package/libertas/src/scan.c b/package/libertas/src/scan.c
index 92d84c72e2..9a61188b62 100644
--- a/package/libertas/src/scan.c
+++ b/package/libertas/src/scan.c
@@ -590,13 +590,13 @@ int lbs_scan_networks(struct lbs_private *priv,
 	netif_stop_queue(priv->dev);
 	netif_carrier_off(priv->dev);
 	if (priv->mesh_dev) {
-			netif_stop_queue(priv->mesh_dev);
-			netif_carrier_off(priv->mesh_dev);
+		netif_stop_queue(priv->mesh_dev);
+		netif_carrier_off(priv->mesh_dev);
 	}
 
 	/* Prepare to continue an interrupted scan */
 	lbs_deb_scan("chan_count %d, last_scanned_channel %d\n",
-		chan_count, priv->last_scanned_channel);
+		     chan_count, priv->last_scanned_channel);
 	curr_chans = chan_list;
 	/* advance channel list by already-scanned-channels */
 	if (priv->last_scanned_channel > 0) {
@@ -659,11 +659,13 @@ out2:
 out:
 	if (priv->connect_status == LBS_CONNECTED) {
 		netif_carrier_on(priv->dev);
-		netif_wake_queue(priv->dev);
+		if (!priv->tx_pending_len)
+			netif_wake_queue(priv->dev);
 	}
 	if (priv->mesh_dev && (priv->mesh_connect_status == LBS_CONNECTED)) {
 		netif_carrier_on(priv->mesh_dev);
-		netif_wake_queue(priv->mesh_dev);
+		if (!priv->tx_pending_len)
+			netif_wake_queue(priv->mesh_dev);
 	}
 	kfree(chan_list);
 
diff --git a/package/libertas/src/tx.c b/package/libertas/src/tx.c
index 8a1a3965f1..00d95f75bd 100644
--- a/package/libertas/src/tx.c
+++ b/package/libertas/src/tx.c
@@ -93,8 +93,8 @@ int lbs_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
 		netif_stop_queue(priv->mesh_dev);
 
 	if (priv->tx_pending_len) {
-		/* This can happen if packets come in on the mesh and eth 
-		   device simultaneously -- there's no mutual exclusion on 
+		/* This can happen if packets come in on the mesh and eth
+		   device simultaneously -- there's no mutual exclusion on
 		   hard_start_xmit() calls between devices. */
 		lbs_deb_tx("Packet on %s while busy\n", dev->name);
 		ret = NETDEV_TX_BUSY;
diff --git a/package/libertas/src/wext.c b/package/libertas/src/wext.c
index 262d4cc580..3e8d555ba3 100644
--- a/package/libertas/src/wext.c
+++ b/package/libertas/src/wext.c
@@ -734,6 +734,13 @@ static int lbs_set_power(struct net_device *dev, struct iw_request_info *info,
 
 	lbs_deb_enter(LBS_DEB_WEXT);
 
+	if (!priv->ps_supported) {
+		if (vwrq->disabled)
+			return 0;
+		else
+			return -EINVAL;
+	}
+
 	/* PS is currently supported only in Infrastructure mode
 	 * Remove this check if it is to be supported in IBSS mode also
 	 */
@@ -1000,9 +1007,8 @@ static int lbs_mesh_set_freq(struct net_device *dev,
 		else if (priv->mode == IW_MODE_ADHOC)
 			lbs_stop_adhoc_network(priv);
 	}
-	priv->curbssparams.channel = fwrq->m;
-	lbs_mesh_config(priv, 0);
-	lbs_mesh_config(priv, 1);
+	lbs_mesh_config(priv, 1, fwrq->m);
+	lbs_update_channel(priv);
 	ret = 0;
 
 out:
@@ -2010,7 +2016,7 @@ static int lbs_mesh_set_essid(struct net_device *dev,
 		priv->mesh_ssid_len = dwrq->length;
 	}
 
-	lbs_mesh_config(priv, 1);
+	lbs_mesh_config(priv, 1, priv->curbssparams.channel);
  out:
 	lbs_deb_leave_args(LBS_DEB_WEXT, "ret %d", ret);
 	return ret;