15 files changed, 616 insertions, 1018 deletions

diff --git a/drivers/net/wireless/libertas/cfg.c b/drivers/net/wireless/libertas/cfg.c
index 7e0741608856..25f902760980 100644
--- a/drivers/net/wireless/libertas/cfg.c
+++ b/drivers/net/wireless/libertas/cfg.c
@@ -7,7 +7,6 @@
  */
 
 #include <linux/slab.h>
-#include <linux/if_arp.h>
 #include <linux/ieee80211.h>
 #include <net/cfg80211.h>
 #include <asm/unaligned.h>
@@ -1383,93 +1382,10 @@ static int lbs_cfg_del_key(struct wiphy *wiphy, struct net_device *netdev,
 }
 
 
-
-/***************************************************************************
- * Monitor mode
- */
-
-/* like "struct cmd_ds_802_11_monitor_mode", but with cmd_header. Once we
- * get rid of WEXT, this should go into host.h */
-struct cmd_monitor_mode {
-	struct cmd_header hdr;
-
-	__le16 action;
-	__le16 mode;
-} __packed;
-
-static int lbs_enable_monitor_mode(struct lbs_private *priv, int mode)
-{
-	struct cmd_monitor_mode cmd;
-	int ret;
-
-	lbs_deb_enter(LBS_DEB_CFG80211);
-
-	/*
-	 * cmd       98 00
-	 * size      0c 00
-	 * sequence  xx xx
-	 * result    00 00
-	 * action    01 00    ACT_SET
-	 * enable    01 00
-	 */
-	memset(&cmd, 0, sizeof(cmd));
-	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
-	cmd.action = cpu_to_le16(CMD_ACT_SET);
-	cmd.mode = cpu_to_le16(mode);
-
-	ret = lbs_cmd_with_response(priv, CMD_802_11_MONITOR_MODE, &cmd);
-
-	if (ret == 0)
-		priv->dev->type = ARPHRD_IEEE80211_RADIOTAP;
-	else
-		priv->dev->type = ARPHRD_ETHER;
-
-	lbs_deb_leave(LBS_DEB_CFG80211);
-	return ret;
-}
-
-
-
-
-
-
 /***************************************************************************
  * Get station
  */
 
-/*
- * Returns the signal or 0 in case of an error.
- */
-
-/* like "struct cmd_ds_802_11_rssi", but with cmd_header. Once we get rid
- * of WEXT, this should go into host.h */
-struct cmd_rssi {
-	struct cmd_header hdr;
-
-	__le16 n_or_snr;
-	__le16 nf;
-	__le16 avg_snr;
-	__le16 avg_nf;
-} __packed;
-
-static int lbs_get_signal(struct lbs_private *priv, s8 *signal, s8 *noise)
-{
-	struct cmd_rssi cmd;
-	int ret;
-
-	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
-	cmd.n_or_snr = cpu_to_le16(DEFAULT_BCN_AVG_FACTOR);
-	ret = lbs_cmd_with_response(priv, CMD_802_11_RSSI, &cmd);
-
-	if (ret == 0) {
-		*signal = CAL_RSSI(le16_to_cpu(cmd.n_or_snr),
-				le16_to_cpu(cmd.nf));
-		*noise  = CAL_NF(le16_to_cpu(cmd.nf));
-	}
-	return ret;
-}
-
-
 static int lbs_cfg_get_station(struct wiphy *wiphy, struct net_device *dev,
 			      u8 *mac, struct station_info *sinfo)
 {
@@ -1490,7 +1406,7 @@ static int lbs_cfg_get_station(struct wiphy *wiphy, struct net_device *dev,
 	sinfo->rx_packets = priv->dev->stats.rx_packets;
 
 	/* Get current RSSI */
-	ret = lbs_get_signal(priv, &signal, &noise);
+	ret = lbs_get_rssi(priv, &signal, &noise);
 	if (ret == 0) {
 		sinfo->signal = signal;
 		sinfo->filled |= STATION_INFO_SIGNAL;
@@ -1530,7 +1446,7 @@ static int lbs_get_survey(struct wiphy *wiphy, struct net_device *dev,
 	survey->channel = ieee80211_get_channel(wiphy,
 		ieee80211_channel_to_frequency(priv->channel));
 
-	ret = lbs_get_signal(priv, &signal, &noise);
+	ret = lbs_get_rssi(priv, &signal, &noise);
 	if (ret == 0) {
 		survey->filled = SURVEY_INFO_NOISE_DBM;
 		survey->noise = noise;
@@ -1558,17 +1474,17 @@ static int lbs_change_intf(struct wiphy *wiphy, struct net_device *dev,
 
 	switch (type) {
 	case NL80211_IFTYPE_MONITOR:
-		ret = lbs_enable_monitor_mode(priv, 1);
+		ret = lbs_set_monitor_mode(priv, 1);
 		break;
 	case NL80211_IFTYPE_STATION:
 		if (priv->wdev->iftype == NL80211_IFTYPE_MONITOR)
-			ret = lbs_enable_monitor_mode(priv, 0);
+			ret = lbs_set_monitor_mode(priv, 0);
 		if (!ret)
 			ret = lbs_set_snmp_mib(priv, SNMP_MIB_OID_BSS_TYPE, 1);
 		break;
 	case NL80211_IFTYPE_ADHOC:
 		if (priv->wdev->iftype == NL80211_IFTYPE_MONITOR)
-			ret = lbs_enable_monitor_mode(priv, 0);
+			ret = lbs_set_monitor_mode(priv, 0);
 		if (!ret)
 			ret = lbs_set_snmp_mib(priv, SNMP_MIB_OID_BSS_TYPE, 2);
 		break;
@@ -2063,113 +1979,20 @@ int lbs_cfg_register(struct lbs_private *priv)
 	return ret;
 }
 
-/**
- *  @brief This function sets DOMAIN INFO to FW
- *  @param priv       pointer to struct lbs_private
- *  @return          0; -1
-*/
-static int lbs_11d_set_domain_info(struct lbs_private *priv)
-{
-	int ret;
-
-	ret = lbs_prepare_and_send_command(priv, CMD_802_11D_DOMAIN_INFO,
-			CMD_ACT_SET,
-			CMD_OPTION_WAITFORRSP, 0, NULL);
-	if (ret)
-		lbs_deb_11d("fail to dnld domain info\n");
-
-	return ret;
-}
-
-static void lbs_send_domain_info_cmd_fw(struct wiphy *wiphy,
-					struct regulatory_request *request)
-{
-	u8   no_of_triplet = 0;
-	u8   no_of_parsed_chan = 0;
-	u8   first_channel = 0, next_chan = 0, max_pwr = 0;
-	u8   i, flag = 0;
-	enum ieee80211_band band;
-	struct ieee80211_supported_band *sband;
-	struct ieee80211_channel *ch;
-	struct lbs_private *priv = wiphy_priv(wiphy);
-	struct lbs_802_11d_domain_reg *domain_info = &priv->domain_reg;
-	int ret = 0;
-
-	lbs_deb_enter(LBS_DEB_CFG80211);
-
-	/* Set country code */
-	domain_info->country_code[0] = request->alpha2[0];
-	domain_info->country_code[1] = request->alpha2[1];
-	domain_info->country_code[2] = ' ';
-
-	for (band = 0; band < IEEE80211_NUM_BANDS ; band++) {
-
-		if (!wiphy->bands[band])
-			continue;
-
-		sband = wiphy->bands[band];
-
-		for (i = 0; i < sband->n_channels ; i++) {
-			ch = &sband->channels[i];
-			if (ch->flags & IEEE80211_CHAN_DISABLED)
-				continue;
-
-			if (!flag) {
-				flag = 1;
-				next_chan = first_channel = (u32) ch->hw_value;
-				max_pwr = ch->max_power;
-				no_of_parsed_chan = 1;
-				continue;
-			}
-
-			if (ch->hw_value == next_chan + 1 &&
-					ch->max_power == max_pwr) {
-				next_chan++;
-				no_of_parsed_chan++;
-			} else {
-				domain_info->triplet[no_of_triplet]
-					.chans.first_channel = first_channel;
-				domain_info->triplet[no_of_triplet]
-					.chans.num_channels = no_of_parsed_chan;
-				domain_info->triplet[no_of_triplet]
-					.chans.max_power = max_pwr;
-				no_of_triplet++;
-				flag = 0;
-			}
-		}
-		if (flag) {
-			domain_info->triplet[no_of_triplet]
-				.chans.first_channel = first_channel;
-			domain_info->triplet[no_of_triplet]
-				.chans.num_channels = no_of_parsed_chan;
-			domain_info->triplet[no_of_triplet]
-				.chans.max_power = max_pwr;
-			no_of_triplet++;
-		}
-	}
-
-	domain_info->no_triplet = no_of_triplet;
-
-	/* Set domain info */
-	ret = lbs_11d_set_domain_info(priv);
-	if (ret)
-		lbs_pr_err("11D: error setting domain info in FW\n");
-
-	lbs_deb_leave(LBS_DEB_CFG80211);
-}
-
 int lbs_reg_notifier(struct wiphy *wiphy,
 		struct regulatory_request *request)
 {
+	struct lbs_private *priv = wiphy_priv(wiphy);
+	int ret;
+
 	lbs_deb_enter_args(LBS_DEB_CFG80211, "cfg80211 regulatory domain "
 			"callback for domain %c%c\n", request->alpha2[0],
 			request->alpha2[1]);
 
-	lbs_send_domain_info_cmd_fw(wiphy, request);
+	ret = lbs_set_11d_domain_info(priv, request, wiphy->bands);
 
 	lbs_deb_leave(LBS_DEB_CFG80211);
-
-	return 0;
+	return ret;
 }
 
 void lbs_scan_deinit(struct lbs_private *priv)
diff --git a/drivers/net/wireless/libertas/cfg.h b/drivers/net/wireless/libertas/cfg.h
index 756fb98f9f05..4f46bb744bee 100644
--- a/drivers/net/wireless/libertas/cfg.h
+++ b/drivers/net/wireless/libertas/cfg.h
@@ -13,12 +13,6 @@ void lbs_cfg_free(struct lbs_private *priv);
 int lbs_reg_notifier(struct wiphy *wiphy,
 		struct regulatory_request *request);
 
-/* All of those are TODOs: */
-#define lbs_cmd_802_11_rssi(priv, cmdptr) (0)
-#define lbs_ret_802_11_rssi(priv, resp) (0)
-#define lbs_cmd_bcn_ctrl(priv, cmdptr, cmd_action) (0)
-#define lbs_ret_802_11_bcn_ctrl(priv, resp) (0)
-
 void lbs_send_disconnect_notification(struct lbs_private *priv);
 void lbs_send_mic_failureevent(struct lbs_private *priv, u32 event);
 
diff --git a/drivers/net/wireless/libertas/cmd.c b/drivers/net/wireless/libertas/cmd.c
index 749fbde4fd54..70745928f3f8 100644
--- a/drivers/net/wireless/libertas/cmd.c
+++ b/drivers/net/wireless/libertas/cmd.c
@@ -6,13 +6,14 @@
 #include <linux/kfifo.h>
 #include <linux/sched.h>
 #include <linux/slab.h>
+#include <linux/if_arp.h>
 
 #include "decl.h"
 #include "cfg.h"
 #include "cmd.h"
 
-
-static struct cmd_ctrl_node *lbs_get_cmd_ctrl_node(struct lbs_private *priv);
+#define CAL_NF(nf)		((s32)(-(s32)(nf)))
+#define CAL_RSSI(snr, nf)	((s32)((s32)(snr) + CAL_NF(nf)))
 
 /**
  *  @brief Simple callback that copies response back into command
@@ -74,30 +75,6 @@ static u8 is_command_allowed_in_ps(u16 cmd)
 }
 
 /**
- *  @brief This function checks if the command is allowed.
- *
- *  @param priv         A pointer to lbs_private structure
- *  @return             allowed or not allowed.
- */
-
-static int lbs_is_cmd_allowed(struct lbs_private *priv)
-{
-	int ret = 1;
-
-	lbs_deb_enter(LBS_DEB_CMD);
-
-	if (!priv->is_auto_deep_sleep_enabled) {
-		if (priv->is_deep_sleep) {
-			lbs_deb_cmd("command not allowed in deep sleep\n");
-			ret = 0;
-		}
-	}
-
-	lbs_deb_leave(LBS_DEB_CMD);
-	return ret;
-}
-
-/**
  *  @brief Updates the hardware details like MAC address and regulatory region
  *
  *  @param priv    	A pointer to struct lbs_private structure
@@ -227,42 +204,49 @@ int lbs_host_sleep_cfg(struct lbs_private *priv, uint32_t criteria,
 }
 EXPORT_SYMBOL_GPL(lbs_host_sleep_cfg);
 
-static int lbs_cmd_802_11_ps_mode(struct cmd_ds_command *cmd,
-				   u16 cmd_action)
+/**
+ *  @brief Sets the Power Save mode
+ *
+ *  @param priv    	A pointer to struct lbs_private structure
+ *  @param cmd_action	The Power Save operation (PS_MODE_ACTION_ENTER_PS or
+ *                         PS_MODE_ACTION_EXIT_PS)
+ *  @param block	Whether to block on a response or not
+ *
+ *  @return 	   	0 on success, error on failure
+ */
+int lbs_set_ps_mode(struct lbs_private *priv, u16 cmd_action, bool block)
 {
-	struct cmd_ds_802_11_ps_mode *psm = &cmd->params.psmode;
+	struct cmd_ds_802_11_ps_mode cmd;
+	int ret = 0;
 
 	lbs_deb_enter(LBS_DEB_CMD);
 
-	cmd->command = cpu_to_le16(CMD_802_11_PS_MODE);
-	cmd->size = cpu_to_le16(sizeof(struct cmd_ds_802_11_ps_mode) +
-				sizeof(struct cmd_header));
-	psm->action = cpu_to_le16(cmd_action);
-	psm->multipledtim = 0;
-	switch (cmd_action) {
-	case CMD_SUBCMD_ENTER_PS:
-		lbs_deb_cmd("PS command:" "SubCode- Enter PS\n");
-
-		psm->locallisteninterval = 0;
-		psm->nullpktinterval = 0;
-		psm->multipledtim =
-		    cpu_to_le16(MRVDRV_DEFAULT_MULTIPLE_DTIM);
-		break;
-
-	case CMD_SUBCMD_EXIT_PS:
-		lbs_deb_cmd("PS command:" "SubCode- Exit PS\n");
-		break;
-
-	case CMD_SUBCMD_SLEEP_CONFIRMED:
-		lbs_deb_cmd("PS command: SubCode- sleep confirm\n");
-		break;
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
+	cmd.action = cpu_to_le16(cmd_action);
 
-	default:
-		break;
+	if (cmd_action == PS_MODE_ACTION_ENTER_PS) {
+		lbs_deb_cmd("PS_MODE: action ENTER_PS\n");
+		cmd.multipledtim = cpu_to_le16(1);  /* Default DTIM multiple */
+	} else if (cmd_action == PS_MODE_ACTION_EXIT_PS) {
+		lbs_deb_cmd("PS_MODE: action EXIT_PS\n");
+	} else {
+		/* We don't handle CONFIRM_SLEEP here because it needs to
+		 * be fastpathed to the firmware.
+		 */
+		lbs_deb_cmd("PS_MODE: unknown action 0x%X\n", cmd_action);
+		ret = -EOPNOTSUPP;
+		goto out;
 	}
 
-	lbs_deb_leave(LBS_DEB_CMD);
-	return 0;
+	if (block)
+		ret = lbs_cmd_with_response(priv, CMD_802_11_PS_MODE, &cmd);
+	else
+		lbs_cmd_async(priv, CMD_802_11_PS_MODE, &cmd.hdr, sizeof (cmd));
+
+out:
+	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
+	return ret;
 }
 
 int lbs_cmd_802_11_sleep_params(struct lbs_private *priv, uint16_t cmd_action,
@@ -576,23 +560,35 @@ int lbs_set_tx_power(struct lbs_private *priv, s16 dbm)
 	return ret;
 }
 
-static int lbs_cmd_802_11_monitor_mode(struct cmd_ds_command *cmd,
-				      u16 cmd_action, void *pdata_buf)
+/**
+ *  @brief Enable or disable monitor mode (only implemented on OLPC usb8388 FW)
+ *
+ *  @param priv        A pointer to struct lbs_private structure
+ *  @param enable      1 to enable monitor mode, 0 to disable
+ *
+ *  @return            0 on success, error on failure
+ */
+int lbs_set_monitor_mode(struct lbs_private *priv, int enable)
 {
-	struct cmd_ds_802_11_monitor_mode *monitor = &cmd->params.monitor;
+	struct cmd_ds_802_11_monitor_mode cmd;
+	int ret;
 
-	cmd->command = cpu_to_le16(CMD_802_11_MONITOR_MODE);
-	cmd->size =
-	    cpu_to_le16(sizeof(struct cmd_ds_802_11_monitor_mode) +
-			     sizeof(struct cmd_header));
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
+	cmd.action = cpu_to_le16(CMD_ACT_SET);
+	if (enable)
+		cmd.mode = cpu_to_le16(0x1);
+
+	lbs_deb_cmd("SET_MONITOR_MODE: %d\n", enable);
 
-	monitor->action = cpu_to_le16(cmd_action);
-	if (cmd_action == CMD_ACT_SET) {
-		monitor->mode =
-		    cpu_to_le16((u16) (*(u32 *) pdata_buf));
+	ret = lbs_cmd_with_response(priv, CMD_802_11_MONITOR_MODE, &cmd);
+	if (ret == 0) {
+		priv->dev->type = enable ? ARPHRD_IEEE80211_RADIOTAP :
+						ARPHRD_ETHER;
 	}
 
-	return 0;
+	lbs_deb_leave(LBS_DEB_CMD);
+	return ret;
 }
 
 /**
@@ -677,78 +673,242 @@ out:
 	return ret;
 }
 
-static int lbs_cmd_reg_access(struct cmd_ds_command *cmdptr,
-			       u8 cmd_action, void *pdata_buf)
+/**
+ *  @brief Get current RSSI and noise floor
+ *
+ *  @param priv		A pointer to struct lbs_private structure
+ *  @param rssi		On successful return, signal level in mBm
+ *
+ *  @return 	   	The channel on success, error on failure
+ */
+int lbs_get_rssi(struct lbs_private *priv, s8 *rssi, s8 *nf)
 {
-	struct lbs_offset_value *offval;
+	struct cmd_ds_802_11_rssi cmd;
+	int ret = 0;
 
 	lbs_deb_enter(LBS_DEB_CMD);
 
-	offval = (struct lbs_offset_value *)pdata_buf;
+	BUG_ON(rssi == NULL);
+	BUG_ON(nf == NULL);
 
-	switch (le16_to_cpu(cmdptr->command)) {
-	case CMD_MAC_REG_ACCESS:
-		{
-			struct cmd_ds_mac_reg_access *macreg;
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
+	/* Average SNR over last 8 beacons */
+	cmd.n_or_snr = cpu_to_le16(8);
 
-			cmdptr->size =
-			    cpu_to_le16(sizeof (struct cmd_ds_mac_reg_access)
-					+ sizeof(struct cmd_header));
-			macreg =
-			    (struct cmd_ds_mac_reg_access *)&cmdptr->params.
-			    macreg;
+	ret = lbs_cmd_with_response(priv, CMD_802_11_RSSI, &cmd);
+	if (ret == 0) {
+		*nf = CAL_NF(le16_to_cpu(cmd.nf));
+		*rssi = CAL_RSSI(le16_to_cpu(cmd.n_or_snr), le16_to_cpu(cmd.nf));
+	}
 
-			macreg->action = cpu_to_le16(cmd_action);
-			macreg->offset = cpu_to_le16((u16) offval->offset);
-			macreg->value = cpu_to_le32(offval->value);
+	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
+	return ret;
+}
 
-			break;
-		}
+/**
+ *  @brief Send regulatory and 802.11d domain information to the firmware
+ *
+ *  @param priv		pointer to struct lbs_private
+ *  @param request	cfg80211 regulatory request structure
+ *  @param bands	the device's supported bands and channels
+ *
+ *  @return		0 on success, error code on failure
+*/
+int lbs_set_11d_domain_info(struct lbs_private *priv,
+			    struct regulatory_request *request,
+			    struct ieee80211_supported_band **bands)
+{
+	struct cmd_ds_802_11d_domain_info cmd;
+	struct mrvl_ie_domain_param_set *domain = &cmd.domain;
+	struct ieee80211_country_ie_triplet *t;
+	enum ieee80211_band band;
+	struct ieee80211_channel *ch;
+	u8 num_triplet = 0;
+	u8 num_parsed_chan = 0;
+	u8 first_channel = 0, next_chan = 0, max_pwr = 0;
+	u8 i, flag = 0;
+	size_t triplet_size;
+	int ret;
 
-	case CMD_BBP_REG_ACCESS:
-		{
-			struct cmd_ds_bbp_reg_access *bbpreg;
+	lbs_deb_enter(LBS_DEB_11D);
 
-			cmdptr->size =
-			    cpu_to_le16(sizeof
-					     (struct cmd_ds_bbp_reg_access)
-					     + sizeof(struct cmd_header));
-			bbpreg =
-			    (struct cmd_ds_bbp_reg_access *)&cmdptr->params.
-			    bbpreg;
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.action = cpu_to_le16(CMD_ACT_SET);
 
-			bbpreg->action = cpu_to_le16(cmd_action);
-			bbpreg->offset = cpu_to_le16((u16) offval->offset);
-			bbpreg->value = (u8) offval->value;
+	lbs_deb_11d("Setting country code '%c%c'\n",
+		    request->alpha2[0], request->alpha2[1]);
 
-			break;
+	domain->header.type = cpu_to_le16(TLV_TYPE_DOMAIN);
+
+	/* Set country code */
+	domain->country_code[0] = request->alpha2[0];
+	domain->country_code[1] = request->alpha2[1];
+	domain->country_code[2] = ' ';
+
+	/* Now set up the channel triplets; firmware is somewhat picky here
+	 * and doesn't validate channel numbers and spans; hence it would
+	 * interpret a triplet of (36, 4, 20) as channels 36, 37, 38, 39.  Since
+	 * the last 3 aren't valid channels, the driver is responsible for
+	 * splitting that up into 4 triplet pairs of (36, 1, 20) + (40, 1, 20)
+	 * etc.
+	 */
+	for (band = 0;
+	     (band < IEEE80211_NUM_BANDS) && (num_triplet < MAX_11D_TRIPLETS);
+	     band++) {
+
+		if (!bands[band])
+			continue;
+
+		for (i = 0;
+		     (i < bands[band]->n_channels) && (num_triplet < MAX_11D_TRIPLETS);
+		     i++) {
+			ch = &bands[band]->channels[i];
+			if (ch->flags & IEEE80211_CHAN_DISABLED)
+				continue;
+
+			if (!flag) {
+				flag = 1;
+				next_chan = first_channel = (u32) ch->hw_value;
+				max_pwr = ch->max_power;
+				num_parsed_chan = 1;
+				continue;
+			}
+
+			if ((ch->hw_value == next_chan + 1) &&
+					(ch->max_power == max_pwr)) {
+				/* Consolidate adjacent channels */
+				next_chan++;
+				num_parsed_chan++;
+			} else {
+				/* Add this triplet */
+				lbs_deb_11d("11D triplet (%d, %d, %d)\n",
+					first_channel, num_parsed_chan,
+					max_pwr);
+				t = &domain->triplet[num_triplet];
+				t->chans.first_channel = first_channel;
+				t->chans.num_channels = num_parsed_chan;
+				t->chans.max_power = max_pwr;
+				num_triplet++;
+				flag = 0;
+			}
 		}
 
-	case CMD_RF_REG_ACCESS:
-		{
-			struct cmd_ds_rf_reg_access *rfreg;
+		if (flag) {
+			/* Add last triplet */
+			lbs_deb_11d("11D triplet (%d, %d, %d)\n", first_channel,
+				num_parsed_chan, max_pwr);
+			t = &domain->triplet[num_triplet];
+			t->chans.first_channel = first_channel;
+			t->chans.num_channels = num_parsed_chan;
+			t->chans.max_power = max_pwr;
+			num_triplet++;
+		}
+	}
 
-			cmdptr->size =
-			    cpu_to_le16(sizeof
-					     (struct cmd_ds_rf_reg_access) +
-					     sizeof(struct cmd_header));
-			rfreg =
-			    (struct cmd_ds_rf_reg_access *)&cmdptr->params.
-			    rfreg;
+	lbs_deb_11d("# triplets %d\n", num_triplet);
 
-			rfreg->action = cpu_to_le16(cmd_action);
-			rfreg->offset = cpu_to_le16((u16) offval->offset);
-			rfreg->value = (u8) offval->value;
+	/* Set command header sizes */
+	triplet_size = num_triplet * sizeof(struct ieee80211_country_ie_triplet);
+	domain->header.len = cpu_to_le16(sizeof(domain->country_code) +
+					triplet_size);
 
-			break;
-		}
+	lbs_deb_hex(LBS_DEB_11D, "802.11D domain param set",
+			(u8 *) &cmd.domain.country_code,
+			le16_to_cpu(domain->header.len));
 
-	default:
-		break;
+	cmd.hdr.size = cpu_to_le16(sizeof(cmd.hdr) +
+				   sizeof(cmd.action) +
+				   sizeof(cmd.domain.header) +
+				   sizeof(cmd.domain.country_code) +
+				   triplet_size);
+
+	ret = lbs_cmd_with_response(priv, CMD_802_11D_DOMAIN_INFO, &cmd);
+
+	lbs_deb_leave_args(LBS_DEB_11D, "ret %d", ret);
+	return ret;
+}
+
+/**
+ *  @brief Read a MAC, Baseband, or RF register
+ *
+ *  @param priv		pointer to struct lbs_private
+ *  @param cmd		register command, one of CMD_MAC_REG_ACCESS,
+ *                        CMD_BBP_REG_ACCESS, or CMD_RF_REG_ACCESS
+ *  @param offset       byte offset of the register to get
+ *  @param value        on success, the value of the register at 'offset'
+ *
+ *  @return		0 on success, error code on failure
+*/
+int lbs_get_reg(struct lbs_private *priv, u16 reg, u16 offset, u32 *value)
+{
+	struct cmd_ds_reg_access cmd;
+	int ret = 0;
+
+	lbs_deb_enter(LBS_DEB_CMD);
+
+	BUG_ON(value == NULL);
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
+	cmd.action = cpu_to_le16(CMD_ACT_GET);
+
+	if (reg != CMD_MAC_REG_ACCESS &&
+	    reg != CMD_BBP_REG_ACCESS &&
+	    reg != CMD_RF_REG_ACCESS) {
+		ret = -EINVAL;
+		goto out;
 	}
 
-	lbs_deb_leave(LBS_DEB_CMD);
-	return 0;
+	ret = lbs_cmd_with_response(priv, reg, &cmd);
+	if (ret) {
+		if (reg == CMD_BBP_REG_ACCESS || reg == CMD_RF_REG_ACCESS)
+			*value = cmd.value.bbp_rf;
+		else if (reg == CMD_MAC_REG_ACCESS)
+			*value = le32_to_cpu(cmd.value.mac);
+	}
+
+out:
+	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
+	return ret;
+}
+
+/**
+ *  @brief Write a MAC, Baseband, or RF register
+ *
+ *  @param priv		pointer to struct lbs_private
+ *  @param cmd		register command, one of CMD_MAC_REG_ACCESS,
+ *                        CMD_BBP_REG_ACCESS, or CMD_RF_REG_ACCESS
+ *  @param offset       byte offset of the register to set
+ *  @param value        the value to write to the register at 'offset'
+ *
+ *  @return		0 on success, error code on failure
+*/
+int lbs_set_reg(struct lbs_private *priv, u16 reg, u16 offset, u32 value)
+{
+	struct cmd_ds_reg_access cmd;
+	int ret = 0;
+
+	lbs_deb_enter(LBS_DEB_CMD);
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
+	cmd.action = cpu_to_le16(CMD_ACT_SET);
+
+	if (reg == CMD_BBP_REG_ACCESS || reg == CMD_RF_REG_ACCESS)
+		cmd.value.bbp_rf = (u8) (value & 0xFF);
+	else if (reg == CMD_MAC_REG_ACCESS)
+		cmd.value.mac = cpu_to_le32(value);
+	else {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	ret = lbs_cmd_with_response(priv, reg, &cmd);
+
+out:
+	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
+	return ret;
 }
 
 static void lbs_queue_cmd(struct lbs_private *priv,
@@ -771,16 +931,15 @@ static void lbs_queue_cmd(struct lbs_private *priv,
 
 	/* 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[1];
+		struct cmd_ds_802_11_ps_mode *psm = (void *) &cmdnode->cmdbuf;
 
-		if (psm->action == cpu_to_le16(CMD_SUBCMD_EXIT_PS)) {
+		if (psm->action == cpu_to_le16(PS_MODE_ACTION_EXIT_PS)) {
 			if (priv->psstate != PS_STATE_FULL_POWER)
 				addtail = 0;
 		}
 	}
 
-	if (le16_to_cpu(cmdnode->cmdbuf->command) ==
-			CMD_802_11_WAKEUP_CONFIRM)
+	if (le16_to_cpu(cmdnode->cmdbuf->command) == CMD_802_11_WAKEUP_CONFIRM)
 		addtail = 0;
 
 	spin_lock_irqsave(&priv->driver_lock, flags);
@@ -815,7 +974,6 @@ static void lbs_submit_command(struct lbs_private *priv,
 
 	spin_lock_irqsave(&priv->driver_lock, flags);
 	priv->cur_cmd = cmdnode;
-	priv->cur_cmd_retcode = 0;
 	spin_unlock_irqrestore(&priv->driver_lock, flags);
 
 	cmdsize = le16_to_cpu(cmd->size);
@@ -888,9 +1046,6 @@ static void lbs_cleanup_and_insert_cmd(struct lbs_private *priv,
 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);
@@ -958,240 +1113,6 @@ void lbs_set_mac_control(struct lbs_private *priv)
 }
 
 /**
- *  @brief This function implements command CMD_802_11D_DOMAIN_INFO
- *  @param priv       pointer to struct lbs_private
- *  @param cmd        pointer to cmd buffer
- *  @param cmdno      cmd ID
- *  @param cmdOption  cmd action
- *  @return           0
-*/
-int lbs_cmd_802_11d_domain_info(struct lbs_private *priv,
-				 struct cmd_ds_command *cmd,
-				 u16 cmdoption)
-{
-	struct cmd_ds_802_11d_domain_info *pdomaininfo =
-	    &cmd->params.domaininfo;
-	struct mrvl_ie_domain_param_set *domain = &pdomaininfo->domain;
-	u8 nr_triplet = priv->domain_reg.no_triplet;
-
-	lbs_deb_enter(LBS_DEB_11D);
-
-	lbs_deb_11d("nr_triplet=%x\n", nr_triplet);
-
-	pdomaininfo->action = cpu_to_le16(cmdoption);
-	if (cmdoption == CMD_ACT_GET) {
-		cmd->size = cpu_to_le16(sizeof(pdomaininfo->action) +
-					sizeof(struct cmd_header));
-		lbs_deb_hex(LBS_DEB_11D, "802_11D_DOMAIN_INFO", (u8 *) cmd,
-			le16_to_cpu(cmd->size));
-		goto done;
-	}
-
-	domain->header.type = cpu_to_le16(TLV_TYPE_DOMAIN);
-	memcpy(domain->countrycode, priv->domain_reg.country_code,
-	       sizeof(domain->countrycode));
-
-	domain->header.len = cpu_to_le16(nr_triplet
-				* sizeof(struct ieee80211_country_ie_triplet)
-				+ sizeof(domain->countrycode));
-
-	if (nr_triplet) {
-		memcpy(domain->triplet, priv->domain_reg.triplet,
-				nr_triplet *
-				sizeof(struct ieee80211_country_ie_triplet));
-
-		cmd->size = cpu_to_le16(sizeof(pdomaininfo->action) +
-					     le16_to_cpu(domain->header.len) +
-					     sizeof(struct mrvl_ie_header) +
-					     sizeof(struct cmd_header));
-	} else {
-		cmd->size = cpu_to_le16(sizeof(pdomaininfo->action) +
-					sizeof(struct cmd_header));
-	}
-
-	lbs_deb_hex(LBS_DEB_11D, "802_11D_DOMAIN_INFO", (u8 *) cmd,
-			le16_to_cpu(cmd->size));
-
-done:
-	lbs_deb_enter(LBS_DEB_11D);
-	return 0;
-}
-
-/**
- *  @brief This function prepare the command before send to firmware.
- *
- *  @param priv		A pointer to struct lbs_private structure
- *  @param cmd_no	command number
- *  @param cmd_action	command action: GET or SET
- *  @param wait_option	wait option: wait response or not
- *  @param cmd_oid	cmd oid: treated as sub command
- *  @param pdata_buf	A pointer to informaion buffer
- *  @return 		0 or -1
- */
-int lbs_prepare_and_send_command(struct lbs_private *priv,
-			  u16 cmd_no,
-			  u16 cmd_action,
-			  u16 wait_option, u32 cmd_oid, void *pdata_buf)
-{
-	int ret = 0;
-	struct cmd_ctrl_node *cmdnode;
-	struct cmd_ds_command *cmdptr;
-	unsigned long flags;
-
-	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;
-		goto done;
-	}
-
-	if (!lbs_is_cmd_allowed(priv)) {
-		ret = -EBUSY;
-		goto done;
-	}
-
-	cmdnode = lbs_get_cmd_ctrl_node(priv);
-
-	if (cmdnode == NULL) {
-		lbs_deb_host("PREP_CMD: cmdnode is NULL\n");
-
-		/* Wake up main thread to execute next command */
-		wake_up_interruptible(&priv->waitq);
-		ret = -1;
-		goto done;
-	}
-
-	cmdnode->callback = NULL;
-	cmdnode->callback_arg = (unsigned long)pdata_buf;
-
-	cmdptr = (struct cmd_ds_command *)cmdnode->cmdbuf;
-
-	lbs_deb_host("PREP_CMD: command 0x%04x\n", cmd_no);
-
-	/* Set sequence number, command and INT option */
-	priv->seqnum++;
-	cmdptr->seqnum = cpu_to_le16(priv->seqnum);
-
-	cmdptr->command = cpu_to_le16(cmd_no);
-	cmdptr->result = 0;
-
-	switch (cmd_no) {
-	case CMD_802_11_PS_MODE:
-		ret = lbs_cmd_802_11_ps_mode(cmdptr, cmd_action);
-		break;
-
-	case CMD_MAC_REG_ACCESS:
-	case CMD_BBP_REG_ACCESS:
-	case CMD_RF_REG_ACCESS:
-		ret = lbs_cmd_reg_access(cmdptr, cmd_action, pdata_buf);
-		break;
-
-	case CMD_802_11_MONITOR_MODE:
-		ret = lbs_cmd_802_11_monitor_mode(cmdptr,
-				          cmd_action, pdata_buf);
-		break;
-
-	case CMD_802_11_RSSI:
-		ret = lbs_cmd_802_11_rssi(priv, cmdptr);
-		break;
-
-	case CMD_802_11_SET_AFC:
-	case CMD_802_11_GET_AFC:
-
-		cmdptr->command = cpu_to_le16(cmd_no);
-		cmdptr->size = cpu_to_le16(sizeof(struct cmd_ds_802_11_afc) +
-					   sizeof(struct cmd_header));
-
-		memmove(&cmdptr->params.afc,
-			pdata_buf, sizeof(struct cmd_ds_802_11_afc));
-
-		ret = 0;
-		goto done;
-
-	case CMD_802_11D_DOMAIN_INFO:
-		cmdptr->command = cpu_to_le16(cmd_no);
-		ret = lbs_cmd_802_11d_domain_info(priv, cmdptr, cmd_action);
-		break;
-
-	case CMD_802_11_TPC_CFG:
-		cmdptr->command = cpu_to_le16(CMD_802_11_TPC_CFG);
-		cmdptr->size =
-		    cpu_to_le16(sizeof(struct cmd_ds_802_11_tpc_cfg) +
-				     sizeof(struct cmd_header));
-
-		memmove(&cmdptr->params.tpccfg,
-			pdata_buf, sizeof(struct cmd_ds_802_11_tpc_cfg));
-
-		ret = 0;
-		break;
-
-#ifdef CONFIG_LIBERTAS_MESH
-
-	case CMD_BT_ACCESS:
-		ret = lbs_cmd_bt_access(cmdptr, cmd_action, pdata_buf);
-		break;
-
-	case CMD_FWT_ACCESS:
-		ret = lbs_cmd_fwt_access(cmdptr, cmd_action, pdata_buf);
-		break;
-
-#endif
-
-	case CMD_802_11_BEACON_CTRL:
-		ret = lbs_cmd_bcn_ctrl(priv, cmdptr, cmd_action);
-		break;
-	case CMD_802_11_DEEP_SLEEP:
-		cmdptr->command = cpu_to_le16(CMD_802_11_DEEP_SLEEP);
-		cmdptr->size = cpu_to_le16(sizeof(struct cmd_header));
-		break;
-	default:
-		lbs_pr_err("PREP_CMD: unknown command 0x%04x\n", cmd_no);
-		ret = -1;
-		break;
-	}
-
-	/* return error, since the command preparation failed */
-	if (ret != 0) {
-		lbs_deb_host("PREP_CMD: command preparation failed\n");
-		lbs_cleanup_and_insert_cmd(priv, cmdnode);
-		ret = -1;
-		goto done;
-	}
-
-	cmdnode->cmdwaitqwoken = 0;
-
-	lbs_queue_cmd(priv, cmdnode);
-	wake_up_interruptible(&priv->waitq);
-
-	if (wait_option & CMD_OPTION_WAITFORRSP) {
-		lbs_deb_host("PREP_CMD: wait for response\n");
-		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;
-	}
-	spin_unlock_irqrestore(&priv->driver_lock, flags);
-
-done:
-	lbs_deb_leave_args(LBS_DEB_HOST, "ret %d", ret);
-	return ret;
-}
-
-/**
  *  @brief This function allocates the command buffer and link
  *  it to command free queue.
  *
@@ -1284,7 +1205,7 @@ done:
  *  @param priv		A pointer to struct lbs_private structure
  *  @return cmd_ctrl_node A pointer to cmd_ctrl_node structure or NULL
  */
-static struct cmd_ctrl_node *lbs_get_cmd_ctrl_node(struct lbs_private *priv)
+static struct cmd_ctrl_node *lbs_get_free_cmd_node(struct lbs_private *priv)
 {
 	struct cmd_ctrl_node *tempnode;
 	unsigned long flags;
@@ -1367,10 +1288,10 @@ int lbs_execute_next_command(struct lbs_private *priv)
 			/*
 			 * 1. Non-PS command:
 			 * Queue it. set needtowakeup to TRUE if current state
-			 * is SLEEP, otherwise call lbs_ps_wakeup to send Exit_PS.
-			 * 2. PS command but not Exit_PS:
+			 * is SLEEP, otherwise call send EXIT_PS.
+			 * 2. PS command but not EXIT_PS:
 			 * Ignore it.
-			 * 3. PS command Exit_PS:
+			 * 3. PS command EXIT_PS:
 			 * Set needtowakeup to TRUE if current state is SLEEP,
 			 * otherwise send this command down to firmware
 			 * immediately.
@@ -1384,8 +1305,11 @@ int lbs_execute_next_command(struct lbs_private *priv)
 					/* w/ new scheme, it will not reach here.
 					   since it is blocked in main_thread. */
 					priv->needtowakeup = 1;
-				} else
-					lbs_ps_wakeup(priv, 0);
+				} else {
+					lbs_set_ps_mode(priv,
+							PS_MODE_ACTION_EXIT_PS,
+							false);
+				}
 
 				ret = 0;
 				goto done;
@@ -1400,7 +1324,7 @@ int lbs_execute_next_command(struct lbs_private *priv)
 				       "EXEC_NEXT_CMD: PS cmd, action 0x%02x\n",
 				       psm->action);
 				if (psm->action !=
-				    cpu_to_le16(CMD_SUBCMD_EXIT_PS)) {
+				    cpu_to_le16(PS_MODE_ACTION_EXIT_PS)) {
 					lbs_deb_host(
 					       "EXEC_NEXT_CMD: ignore ENTER_PS cmd\n");
 					list_del(&cmdnode->list);
@@ -1460,13 +1384,16 @@ int lbs_execute_next_command(struct lbs_private *priv)
 					lbs_deb_host(
 					       "EXEC_NEXT_CMD: WPA enabled and GTK_SET"
 					       " go back to PS_SLEEP");
-					lbs_ps_sleep(priv, 0);
+					lbs_set_ps_mode(priv,
+							PS_MODE_ACTION_ENTER_PS,
+							false);
 				}
 			} else {
 				lbs_deb_host(
 				       "EXEC_NEXT_CMD: cmdpendingq empty, "
 				       "go back to PS_SLEEP");
-				lbs_ps_sleep(priv, 0);
+				lbs_set_ps_mode(priv, PS_MODE_ACTION_ENTER_PS,
+						false);
 			}
 		}
 #endif
@@ -1514,43 +1441,6 @@ out:
 	lbs_deb_leave(LBS_DEB_HOST);
 }
 
-void lbs_ps_sleep(struct lbs_private *priv, int wait_option)
-{
-	lbs_deb_enter(LBS_DEB_HOST);
-
-	/*
-	 * PS is currently supported only in Infrastructure mode
-	 * Remove this check if it is to be supported in IBSS mode also
-	 */
-
-	lbs_prepare_and_send_command(priv, CMD_802_11_PS_MODE,
-			      CMD_SUBCMD_ENTER_PS, wait_option, 0, NULL);
-
-	lbs_deb_leave(LBS_DEB_HOST);
-}
-
-/**
- *  @brief This function sends Exit_PS command to firmware.
- *
- *  @param priv    	A pointer to struct lbs_private structure
- *  @param wait_option	wait response or not
- *  @return 	   	n/a
- */
-void lbs_ps_wakeup(struct lbs_private *priv, int wait_option)
-{
-	__le32 Localpsmode;
-
-	lbs_deb_enter(LBS_DEB_HOST);
-
-	Localpsmode = cpu_to_le32(LBS802_11POWERMODECAM);
-
-	lbs_prepare_and_send_command(priv, CMD_802_11_PS_MODE,
-			      CMD_SUBCMD_EXIT_PS,
-			      wait_option, 0, &Localpsmode);
-
-	lbs_deb_leave(LBS_DEB_HOST);
-}
-
 /**
  *  @brief This function checks condition and prepares to
  *  send sleep confirm command to firmware if ok.
@@ -1675,12 +1565,18 @@ struct cmd_ctrl_node *__lbs_cmd_async(struct lbs_private *priv,
 		goto done;
 	}
 
-	if (!lbs_is_cmd_allowed(priv)) {
-		cmdnode = ERR_PTR(-EBUSY);
-		goto done;
+	/* No commands are allowed in Deep Sleep until we toggle the GPIO
+	 * to wake up the card and it has signaled that it's ready.
+	 */
+	if (!priv->is_auto_deep_sleep_enabled) {
+		if (priv->is_deep_sleep) {
+			lbs_deb_cmd("command not allowed in deep sleep\n");
+			cmdnode = ERR_PTR(-EBUSY);
+			goto done;
+		}
 	}
 
-	cmdnode = lbs_get_cmd_ctrl_node(priv);
+	cmdnode = lbs_get_free_cmd_node(priv);
 	if (cmdnode == NULL) {
 		lbs_deb_host("PREP_CMD: cmdnode is NULL\n");
 
diff --git a/drivers/net/wireless/libertas/cmd.h b/drivers/net/wireless/libertas/cmd.h
index 386e565d99ad..7109d6b717ea 100644
--- a/drivers/net/wireless/libertas/cmd.h
+++ b/drivers/net/wireless/libertas/cmd.h
@@ -3,6 +3,8 @@
 #ifndef _LBS_CMD_H_
 #define _LBS_CMD_H_
 
+#include <net/cfg80211.h>
+
 #include "host.h"
 #include "dev.h"
 
@@ -37,11 +39,6 @@ struct cmd_ctrl_node {
 #define lbs_cmd_with_response(priv, cmdnr, cmd)	\
 	lbs_cmd(priv, cmdnr, cmd, lbs_cmd_copyback, (unsigned long) (cmd))
 
-int lbs_prepare_and_send_command(struct lbs_private *priv,
-	u16 cmd_no,
-	u16 cmd_action,
-	u16 wait_option, u32 cmd_oid, void *pdata_buf);
-
 void lbs_cmd_async(struct lbs_private *priv, uint16_t command,
 	struct cmd_header *in_cmd, int in_cmd_size);
 
@@ -92,10 +89,6 @@ int lbs_host_sleep_cfg(struct lbs_private *priv, uint32_t criteria,
 int lbs_cmd_802_11_sleep_params(struct lbs_private *priv, uint16_t cmd_action,
 				struct sleep_params *sp);
 
-void lbs_ps_sleep(struct lbs_private *priv, int wait_option);
-
-void lbs_ps_wakeup(struct lbs_private *priv, int wait_option);
-
 void lbs_ps_confirm_sleep(struct lbs_private *priv);
 
 int lbs_set_radio(struct lbs_private *priv, u8 preamble, u8 radio_on);
@@ -129,4 +122,18 @@ int lbs_set_deep_sleep(struct lbs_private *priv, int deep_sleep);
 
 int lbs_set_host_sleep(struct lbs_private *priv, int host_sleep);
 
+int lbs_set_monitor_mode(struct lbs_private *priv, int enable);
+
+int lbs_get_rssi(struct lbs_private *priv, s8 *snr, s8 *nf);
+
+int lbs_set_11d_domain_info(struct lbs_private *priv,
+			    struct regulatory_request *request,
+			    struct ieee80211_supported_band **bands);
+
+int lbs_get_reg(struct lbs_private *priv, u16 reg, u16 offset, u32 *value);
+
+int lbs_set_reg(struct lbs_private *priv, u16 reg, u16 offset, u32 value);
+
+int lbs_set_ps_mode(struct lbs_private *priv, u16 cmd_action, bool block);
+
 #endif /* _LBS_CMD_H */
diff --git a/drivers/net/wireless/libertas/cmdresp.c b/drivers/net/wireless/libertas/cmdresp.c
index a0d9482ef5e2..5e95da9dcc2e 100644
--- a/drivers/net/wireless/libertas/cmdresp.c
+++ b/drivers/net/wireless/libertas/cmdresp.c
@@ -49,171 +49,11 @@ void lbs_mac_event_disconnected(struct lbs_private *priv)
 	if (priv->psstate != PS_STATE_FULL_POWER) {
 		/* make firmware to exit PS mode */
 		lbs_deb_cmd("disconnected, so exit PS mode\n");
-		lbs_ps_wakeup(priv, 0);
+		lbs_set_ps_mode(priv, PS_MODE_ACTION_EXIT_PS, false);
 	}
 	lbs_deb_leave(LBS_DEB_ASSOC);
 }
 
-static int lbs_ret_reg_access(struct lbs_private *priv,
-			       u16 type, struct cmd_ds_command *resp)
-{
-	int ret = 0;
-
-	lbs_deb_enter(LBS_DEB_CMD);
-
-	switch (type) {
-	case CMD_RET(CMD_MAC_REG_ACCESS):
-		{
-			struct cmd_ds_mac_reg_access *reg = &resp->params.macreg;
-
-			priv->offsetvalue.offset = (u32)le16_to_cpu(reg->offset);
-			priv->offsetvalue.value = le32_to_cpu(reg->value);
-			break;
-		}
-
-	case CMD_RET(CMD_BBP_REG_ACCESS):
-		{
-			struct cmd_ds_bbp_reg_access *reg = &resp->params.bbpreg;
-
-			priv->offsetvalue.offset = (u32)le16_to_cpu(reg->offset);
-			priv->offsetvalue.value = reg->value;
-			break;
-		}
-
-	case CMD_RET(CMD_RF_REG_ACCESS):
-		{
-			struct cmd_ds_rf_reg_access *reg = &resp->params.rfreg;
-
-			priv->offsetvalue.offset = (u32)le16_to_cpu(reg->offset);
-			priv->offsetvalue.value = reg->value;
-			break;
-		}
-
-	default:
-		ret = -1;
-	}
-
-	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
-	return ret;
-}
-
-/**
- *  @brief This function parses countryinfo from AP and download country info to FW
- *  @param priv    pointer to struct lbs_private
- *  @param resp    pointer to command response buffer
- *  @return        0; -1
- */
-static int lbs_ret_802_11d_domain_info(struct cmd_ds_command *resp)
-{
-	struct cmd_ds_802_11d_domain_info *domaininfo =
-			&resp->params.domaininforesp;
-	struct mrvl_ie_domain_param_set *domain = &domaininfo->domain;
-	u16 action = le16_to_cpu(domaininfo->action);
-	s16 ret = 0;
-	u8 nr_triplet = 0;
-
-	lbs_deb_enter(LBS_DEB_11D);
-
-	lbs_deb_hex(LBS_DEB_11D, "domain info resp", (u8 *) resp,
-			(int)le16_to_cpu(resp->size));
-
-	nr_triplet = (le16_to_cpu(domain->header.len) - COUNTRY_CODE_LEN) /
-		sizeof(struct ieee80211_country_ie_triplet);
-
-	lbs_deb_11d("domain info resp: nr_triplet %d\n", nr_triplet);
-
-	if (nr_triplet > MRVDRV_MAX_TRIPLET_802_11D) {
-		lbs_deb_11d("invalid number of triplets returned!!\n");
-		return -1;
-	}
-
-	switch (action) {
-	case CMD_ACT_SET:	/*Proc set action */
-		break;
-
-	case CMD_ACT_GET:
-		break;
-	default:
-		lbs_deb_11d("invalid action:%d\n", domaininfo->action);
-		ret = -1;
-		break;
-	}
-
-	lbs_deb_leave_args(LBS_DEB_11D, "ret %d", ret);
-	return ret;
-}
-
-static inline int handle_cmd_response(struct lbs_private *priv,
-				      struct cmd_header *cmd_response)
-{
-	struct cmd_ds_command *resp = (struct cmd_ds_command *) cmd_response;
-	int ret = 0;
-	unsigned long flags;
-	uint16_t respcmd = le16_to_cpu(resp->command);
-
-	lbs_deb_enter(LBS_DEB_HOST);
-
-	switch (respcmd) {
-	case CMD_RET(CMD_MAC_REG_ACCESS):
-	case CMD_RET(CMD_BBP_REG_ACCESS):
-	case CMD_RET(CMD_RF_REG_ACCESS):
-		ret = lbs_ret_reg_access(priv, respcmd, resp);
-		break;
-
-	case CMD_RET(CMD_802_11_SET_AFC):
-	case CMD_RET(CMD_802_11_GET_AFC):
-		spin_lock_irqsave(&priv->driver_lock, flags);
-		memmove((void *)priv->cur_cmd->callback_arg, &resp->params.afc,
-			sizeof(struct cmd_ds_802_11_afc));
-		spin_unlock_irqrestore(&priv->driver_lock, flags);
-
-		break;
-
-	case CMD_RET(CMD_802_11_BEACON_STOP):
-		break;
-
-	case CMD_RET(CMD_802_11_RSSI):
-		ret = lbs_ret_802_11_rssi(priv, resp);
-		break;
-
-	case CMD_RET(CMD_802_11D_DOMAIN_INFO):
-		ret = lbs_ret_802_11d_domain_info(resp);
-		break;
-
-	case CMD_RET(CMD_802_11_TPC_CFG):
-		spin_lock_irqsave(&priv->driver_lock, flags);
-		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_BT_ACCESS):
-		spin_lock_irqsave(&priv->driver_lock, flags);
-		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->callback_arg)
-			memcpy((void *)priv->cur_cmd->callback_arg, &resp->params.fwt,
-			       sizeof(resp->params.fwt));
-		spin_unlock_irqrestore(&priv->driver_lock, flags);
-		break;
-	case CMD_RET(CMD_802_11_BEACON_CTRL):
-		ret = lbs_ret_802_11_bcn_ctrl(priv, resp);
-		break;
-
-	default:
-		lbs_pr_err("CMD_RESP: unknown cmd response 0x%04x\n",
-			   le16_to_cpu(resp->command));
-		break;
-	}
-	lbs_deb_leave(LBS_DEB_HOST);
-	return ret;
-}
-
 int lbs_process_command_response(struct lbs_private *priv, u8 *data, u32 len)
 {
 	uint16_t respcmd, curcmd;
@@ -272,9 +112,6 @@ int lbs_process_command_response(struct lbs_private *priv, u8 *data, u32 len)
 	del_timer(&priv->command_timer);
 	priv->cmd_timed_out = 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[1];
 		u16 action = le16_to_cpu(psmode->action);
@@ -292,9 +129,9 @@ int lbs_process_command_response(struct lbs_private *priv, u8 *data, u32 len)
 			 * lbs_execute_next_command().
 			 */
 			if (priv->wdev->iftype == NL80211_IFTYPE_MONITOR &&
-			    action == CMD_SUBCMD_ENTER_PS)
+			    action == PS_MODE_ACTION_ENTER_PS)
 				priv->psmode = LBS802_11POWERMODECAM;
-		} else if (action == CMD_SUBCMD_ENTER_PS) {
+		} else if (action == PS_MODE_ACTION_ENTER_PS) {
 			priv->needtowakeup = 0;
 			priv->psstate = PS_STATE_AWAKE;
 
@@ -309,11 +146,12 @@ int lbs_process_command_response(struct lbs_private *priv, u8 *data, u32 len)
 
 				spin_unlock_irqrestore(&priv->driver_lock, flags);
 				mutex_unlock(&priv->lock);
-				lbs_ps_wakeup(priv, 0);
+				lbs_set_ps_mode(priv, PS_MODE_ACTION_EXIT_PS,
+						false);
 				mutex_lock(&priv->lock);
 				spin_lock_irqsave(&priv->driver_lock, flags);
 			}
-		} else if (action == CMD_SUBCMD_EXIT_PS) {
+		} else if (action == PS_MODE_ACTION_EXIT_PS) {
 			priv->needtowakeup = 0;
 			priv->psstate = PS_STATE_FULL_POWER;
 			lbs_deb_host("CMD_RESP: EXIT_PS command response\n");
@@ -354,8 +192,7 @@ int lbs_process_command_response(struct lbs_private *priv, u8 *data, u32 len)
 	if (priv->cur_cmd && priv->cur_cmd->callback) {
 		ret = priv->cur_cmd->callback(priv, priv->cur_cmd->callback_arg,
 				resp);
-	} else
-		ret = handle_cmd_response(priv, resp);
+	}
 
 	spin_lock_irqsave(&priv->driver_lock, flags);
 
@@ -452,7 +289,7 @@ int lbs_process_event(struct lbs_private *priv, u32 event)
 			 * in lbs_ps_wakeup()
 			 */
 			lbs_deb_cmd("waking up ...\n");
-			lbs_ps_wakeup(priv, 0);
+			lbs_set_ps_mode(priv, PS_MODE_ACTION_EXIT_PS, false);
 		}
 		break;
 
diff --git a/drivers/net/wireless/libertas/debugfs.c b/drivers/net/wireless/libertas/debugfs.c
index 3db621b18a2f..651a79c8de8a 100644
--- a/drivers/net/wireless/libertas/debugfs.c
+++ b/drivers/net/wireless/libertas/debugfs.c
@@ -446,30 +446,24 @@ static ssize_t lbs_bcnmiss_write(struct file *file, const char __user *userbuf,
 }
 
 
-
 static ssize_t lbs_rdmac_read(struct file *file, char __user *userbuf,
 				  size_t count, loff_t *ppos)
 {
 	struct lbs_private *priv = file->private_data;
-	struct lbs_offset_value offval;
 	ssize_t pos = 0;
 	int ret;
 	unsigned long addr = get_zeroed_page(GFP_KERNEL);
 	char *buf = (char *)addr;
+	u32 val = 0;
+
 	if (!buf)
 		return -ENOMEM;
 
-	offval.offset = priv->mac_offset;
-	offval.value = 0;
-
-	ret = lbs_prepare_and_send_command(priv,
-				CMD_MAC_REG_ACCESS, 0,
-				CMD_OPTION_WAITFORRSP, 0, &offval);
+	ret = lbs_get_reg(priv, CMD_MAC_REG_ACCESS, priv->mac_offset, &val);
 	mdelay(10);
 	if (!ret) {
-		pos += snprintf(buf+pos, len-pos, "MAC[0x%x] = 0x%08x\n",
-				priv->mac_offset, priv->offsetvalue.value);
-
+		pos = snprintf(buf, len, "MAC[0x%x] = 0x%08x\n",
+				priv->mac_offset, val);
 		ret = simple_read_from_buffer(userbuf, count, ppos, buf, pos);
 	}
 	free_page(addr);
@@ -507,7 +501,6 @@ static ssize_t lbs_wrmac_write(struct file *file,
 	struct lbs_private *priv = file->private_data;
 	ssize_t res, buf_size;
 	u32 offset, value;
-	struct lbs_offset_value offval;
 	unsigned long addr = get_zeroed_page(GFP_KERNEL);
 	char *buf = (char *)addr;
 	if (!buf)
@@ -524,11 +517,7 @@ static ssize_t lbs_wrmac_write(struct file *file,
 		goto out_unlock;
 	}
 
-	offval.offset = offset;
-	offval.value = value;
-	res = lbs_prepare_and_send_command(priv,
-				CMD_MAC_REG_ACCESS, 1,
-				CMD_OPTION_WAITFORRSP, 0, &offval);
+	res = lbs_set_reg(priv, CMD_MAC_REG_ACCESS, offset, value);
 	mdelay(10);
 
 	if (!res)
@@ -542,25 +531,20 @@ static ssize_t lbs_rdbbp_read(struct file *file, char __user *userbuf,
 				  size_t count, loff_t *ppos)
 {
 	struct lbs_private *priv = file->private_data;
-	struct lbs_offset_value offval;
 	ssize_t pos = 0;
 	int ret;
 	unsigned long addr = get_zeroed_page(GFP_KERNEL);
 	char *buf = (char *)addr;
+	u32 val;
+
 	if (!buf)
 		return -ENOMEM;
 
-	offval.offset = priv->bbp_offset;
-	offval.value = 0;
-
-	ret = lbs_prepare_and_send_command(priv,
-				CMD_BBP_REG_ACCESS, 0,
-				CMD_OPTION_WAITFORRSP, 0, &offval);
+	ret = lbs_get_reg(priv, CMD_BBP_REG_ACCESS, priv->bbp_offset, &val);
 	mdelay(10);
 	if (!ret) {
-		pos += snprintf(buf+pos, len-pos, "BBP[0x%x] = 0x%08x\n",
-				priv->bbp_offset, priv->offsetvalue.value);
-
+		pos = snprintf(buf, len, "BBP[0x%x] = 0x%08x\n",
+				priv->bbp_offset, val);
 		ret = simple_read_from_buffer(userbuf, count, ppos, buf, pos);
 	}
 	free_page(addr);
@@ -599,7 +583,6 @@ static ssize_t lbs_wrbbp_write(struct file *file,
 	struct lbs_private *priv = file->private_data;
 	ssize_t res, buf_size;
 	u32 offset, value;
-	struct lbs_offset_value offval;
 	unsigned long addr = get_zeroed_page(GFP_KERNEL);
 	char *buf = (char *)addr;
 	if (!buf)
@@ -616,11 +599,7 @@ static ssize_t lbs_wrbbp_write(struct file *file,
 		goto out_unlock;
 	}
 
-	offval.offset = offset;
-	offval.value = value;
-	res = lbs_prepare_and_send_command(priv,
-				CMD_BBP_REG_ACCESS, 1,
-				CMD_OPTION_WAITFORRSP, 0, &offval);
+	res = lbs_set_reg(priv, CMD_BBP_REG_ACCESS, offset, value);
 	mdelay(10);
 
 	if (!res)
@@ -634,25 +613,20 @@ static ssize_t lbs_rdrf_read(struct file *file, char __user *userbuf,
 				  size_t count, loff_t *ppos)
 {
 	struct lbs_private *priv = file->private_data;
-	struct lbs_offset_value offval;
 	ssize_t pos = 0;
 	int ret;
 	unsigned long addr = get_zeroed_page(GFP_KERNEL);
 	char *buf = (char *)addr;
+	u32 val;
+
 	if (!buf)
 		return -ENOMEM;
 
-	offval.offset = priv->rf_offset;
-	offval.value = 0;
-
-	ret = lbs_prepare_and_send_command(priv,
-				CMD_RF_REG_ACCESS, 0,
-				CMD_OPTION_WAITFORRSP, 0, &offval);
+	ret = lbs_get_reg(priv, CMD_RF_REG_ACCESS, priv->rf_offset, &val);
 	mdelay(10);
 	if (!ret) {
-		pos += snprintf(buf+pos, len-pos, "RF[0x%x] = 0x%08x\n",
-				priv->rf_offset, priv->offsetvalue.value);
-
+		pos = snprintf(buf, len, "RF[0x%x] = 0x%08x\n",
+				priv->rf_offset, val);
 		ret = simple_read_from_buffer(userbuf, count, ppos, buf, pos);
 	}
 	free_page(addr);
@@ -691,7 +665,6 @@ static ssize_t lbs_wrrf_write(struct file *file,
 	struct lbs_private *priv = file->private_data;
 	ssize_t res, buf_size;
 	u32 offset, value;
-	struct lbs_offset_value offval;
 	unsigned long addr = get_zeroed_page(GFP_KERNEL);
 	char *buf = (char *)addr;
 	if (!buf)
@@ -708,11 +681,7 @@ static ssize_t lbs_wrrf_write(struct file *file,
 		goto out_unlock;
 	}
 
-	offval.offset = offset;
-	offval.value = value;
-	res = lbs_prepare_and_send_command(priv,
-				CMD_RF_REG_ACCESS, 1,
-				CMD_OPTION_WAITFORRSP, 0, &offval);
+	res = lbs_set_reg(priv, CMD_RF_REG_ACCESS, offset, value);
 	mdelay(10);
 
 	if (!res)
diff --git a/drivers/net/wireless/libertas/decl.h b/drivers/net/wireless/libertas/decl.h
index ba5438a7ba17..1d141fefd767 100644
--- a/drivers/net/wireless/libertas/decl.h
+++ b/drivers/net/wireless/libertas/decl.h
@@ -53,9 +53,4 @@ int lbs_exit_auto_deep_sleep(struct lbs_private *priv);
 u32 lbs_fw_index_to_data_rate(u8 index);
 u8 lbs_data_rate_to_fw_index(u32 rate);
 
-int lbs_cmd_802_11d_domain_info(struct lbs_private *priv,
-		struct cmd_ds_command *cmd, u16 cmdoption);
-
-int lbs_ret_802_11d_domain_info(struct cmd_ds_command *resp);
-
 #endif
diff --git a/drivers/net/wireless/libertas/defs.h b/drivers/net/wireless/libertas/defs.h
index ea3f10ef4e00..d00c728cec47 100644
--- a/drivers/net/wireless/libertas/defs.h
+++ b/drivers/net/wireless/libertas/defs.h
@@ -172,11 +172,6 @@ static inline void lbs_deb_hex(unsigned int grp, const char *prompt, u8 *buf, in
 #define MRVDRV_MAX_BSS_DESCRIPTS		16
 #define MRVDRV_MAX_REGION_CODE			6
 
-#define MRVDRV_IGNORE_MULTIPLE_DTIM		0xfffe
-#define MRVDRV_MIN_MULTIPLE_DTIM		1
-#define MRVDRV_MAX_MULTIPLE_DTIM		5
-#define MRVDRV_DEFAULT_MULTIPLE_DTIM		1
-
 #define MRVDRV_DEFAULT_LISTEN_INTERVAL		10
 
 #define	MRVDRV_CHANNELS_PER_SCAN		4
@@ -301,19 +296,6 @@ static inline void lbs_deb_hex(unsigned int grp, const char *prompt, u8 *buf, in
 #define	BAND_G			(0x02)
 #define ALL_802_11_BANDS	(BAND_B | BAND_G)
 
-/** MACRO DEFINITIONS */
-#define CAL_NF(NF)			((s32)(-(s32)(NF)))
-#define CAL_RSSI(SNR, NF) 		((s32)((s32)(SNR) + CAL_NF(NF)))
-#define SCAN_RSSI(RSSI)			(0x100 - ((u8)(RSSI)))
-
-#define DEFAULT_BCN_AVG_FACTOR		8
-#define DEFAULT_DATA_AVG_FACTOR		8
-#define AVG_SCALE			100
-#define CAL_AVG_SNR_NF(AVG, SNRNF, N)         \
-                        (((AVG) == 0) ? ((u16)(SNRNF) * AVG_SCALE) : \
-                        ((((int)(AVG) * (N -1)) + ((u16)(SNRNF) * \
-                        AVG_SCALE))  / N))
-
 #define MAX_RATES			14
 
 #define	MAX_LEDS			8
diff --git a/drivers/net/wireless/libertas/dev.h b/drivers/net/wireless/libertas/dev.h
index 4536d9c0ad87..3c7e255e18c7 100644
--- a/drivers/net/wireless/libertas/dev.h
+++ b/drivers/net/wireless/libertas/dev.h
@@ -60,14 +60,10 @@ struct lbs_private {
 	struct dentry *regs_dir;
 	struct dentry *debugfs_regs_files[6];
 
-	/** 11D and domain regulatory data */
-	struct lbs_802_11d_domain_reg domain_reg;
-
 	/* Hardware debugging */
 	u32 mac_offset;
 	u32 bbp_offset;
 	u32 rf_offset;
-	struct lbs_offset_value offsetvalue;
 
 	/* Power management */
 	u16 psmode;
@@ -115,12 +111,10 @@ struct lbs_private {
 	struct cmd_ctrl_node *cur_cmd;
 	struct list_head cmdfreeq;    /* free command buffers */
 	struct list_head cmdpendingq; /* pending command buffers */
-	wait_queue_head_t cmd_pending;
 	struct timer_list command_timer;
 	int cmd_timed_out;
 
 	/* Command responses sent from the hardware to the driver */
-	int cur_cmd_retcode;
 	u8 resp_idx;
 	u8 resp_buf[2][LBS_UPLD_SIZE];
 	u32 resp_len[2];
diff --git a/drivers/net/wireless/libertas/host.h b/drivers/net/wireless/libertas/host.h
index 43d020cd7403..5eac1351a021 100644
--- a/drivers/net/wireless/libertas/host.h
+++ b/drivers/net/wireless/libertas/host.h
@@ -94,11 +94,9 @@
 #define CMD_802_11_BEACON_CTRL                  0x00b0
 
 /* For the IEEE Power Save */
-#define CMD_SUBCMD_ENTER_PS                     0x0030
-#define CMD_SUBCMD_EXIT_PS                      0x0031
-#define CMD_SUBCMD_SLEEP_CONFIRMED              0x0034
-#define CMD_SUBCMD_FULL_POWERDOWN               0x0035
-#define CMD_SUBCMD_FULL_POWERUP                 0x0036
+#define PS_MODE_ACTION_ENTER_PS                 0x0030
+#define PS_MODE_ACTION_EXIT_PS                  0x0031
+#define PS_MODE_ACTION_SLEEP_CONFIRMED          0x0034
 
 #define CMD_ENABLE_RSN                          0x0001
 #define CMD_DISABLE_RSN                         0x0000
@@ -163,11 +161,6 @@
 #define CMD_ACT_SET_TX_FIX_RATE                 0x0001
 #define CMD_ACT_GET_TX_RATE                     0x0002
 
-/* Define action or option for CMD_802_11_PS_MODE */
-#define CMD_TYPE_CAM                            0x0000
-#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
@@ -389,30 +382,22 @@ struct lbs_offset_value {
 	u32 value;
 } __packed;
 
-#define MRVDRV_MAX_TRIPLET_802_11D              83
-
-#define COUNTRY_CODE_LEN                        3
+#define MAX_11D_TRIPLETS	83
 
 struct mrvl_ie_domain_param_set {
 	struct mrvl_ie_header header;
 
-	u8 countrycode[COUNTRY_CODE_LEN];
-	struct ieee80211_country_ie_triplet triplet[1];
+	u8 country_code[3];
+	struct ieee80211_country_ie_triplet triplet[MAX_11D_TRIPLETS];
 } __packed;
 
 struct cmd_ds_802_11d_domain_info {
+	struct cmd_header hdr;
+
 	__le16 action;
 	struct mrvl_ie_domain_param_set domain;
 } __packed;
 
-struct lbs_802_11d_domain_reg {
-	/** Country code*/
-	u8 country_code[COUNTRY_CODE_LEN];
-	/** No. of triplet*/
-	u8 no_triplet;
-	struct ieee80211_country_ie_triplet triplet[MRVDRV_MAX_TRIPLET_802_11D];
-} __packed;
-
 /*
  * Define data structure for CMD_GET_HW_SPEC
  * This structure defines the response for the GET_HW_SPEC command
@@ -575,24 +560,15 @@ struct cmd_ds_802_11_snmp_mib {
 	u8 value[128];
 } __packed;
 
-struct cmd_ds_mac_reg_access {
-	__le16 action;
-	__le16 offset;
-	__le32 value;
-} __packed;
-
-struct cmd_ds_bbp_reg_access {
-	__le16 action;
-	__le16 offset;
-	u8 value;
-	u8 reserved[3];
-} __packed;
+struct cmd_ds_reg_access {
+	struct cmd_header hdr;
 
-struct cmd_ds_rf_reg_access {
 	__le16 action;
 	__le16 offset;
-	u8 value;
-	u8 reserved[3];
+	union {
+		u8 bbp_rf;  /* for BBP and RF registers */
+		__le32 mac; /* for MAC registers */
+	} value;
 } __packed;
 
 struct cmd_ds_802_11_radio_control {
@@ -603,6 +579,8 @@ struct cmd_ds_802_11_radio_control {
 } __packed;
 
 struct cmd_ds_802_11_beacon_control {
+	struct cmd_header hdr;
+
 	__le16 action;
 	__le16 beacon_enable;
 	__le16 beacon_period;
@@ -644,19 +622,19 @@ struct cmd_ds_802_11_rf_channel {
 } __packed;
 
 struct cmd_ds_802_11_rssi {
-	/* weighting factor */
-	__le16 N;
+	struct cmd_header hdr;
 
-	__le16 reserved_0;
-	__le16 reserved_1;
-	__le16 reserved_2;
-} __packed;
+	/* request:  number of beacons (N) to average the SNR and NF over
+	 * response: SNR of most recent beacon
+	 */
+	__le16 n_or_snr;
 
-struct cmd_ds_802_11_rssi_rsp {
-	__le16 SNR;
-	__le16 noisefloor;
-	__le16 avgSNR;
-	__le16 avgnoisefloor;
+	/* The following fields are only set in the response.
+	 * In the request these are reserved and should be set to 0.
+	 */
+	__le16 nf;       /* most recent beacon noise floor */
+	__le16 avg_snr;  /* average SNR weighted by N from request */
+	__le16 avg_nf;   /* average noise floor weighted by N from request */
 } __packed;
 
 struct cmd_ds_802_11_mac_address {
@@ -675,7 +653,10 @@ struct cmd_ds_802_11_rf_tx_power {
 	s8 minlevel;
 } __packed;
 
+/* MONITOR_MODE only exists in OLPC v5 firmware */
 struct cmd_ds_802_11_monitor_mode {
+	struct cmd_header hdr;
+
 	__le16 action;
 	__le16 mode;
 } __packed;
@@ -695,11 +676,35 @@ struct cmd_ds_802_11_fw_wake_method {
 } __packed;
 
 struct cmd_ds_802_11_ps_mode {
+	struct cmd_header hdr;
+
 	__le16 action;
+
+	/* Interval for keepalive in PS mode:
+	 * 0x0000 = don't change
+	 * 0x001E = firmware default
+	 * 0xFFFF = disable
+	 */
 	__le16 nullpktinterval;
+
+	/* Number of DTIM intervals to wake up for:
+	 * 0 = don't change
+	 * 1 = firmware default
+	 * 5 = max
+	 */
 	__le16 multipledtim;
+
 	__le16 reserved;
 	__le16 locallisteninterval;
+
+	/* AdHoc awake period (FW v9+ only):
+	 * 0 = don't change
+	 * 1 = always awake (IEEE standard behavior)
+	 * 2 - 31 = sleep for (n - 1) periods and awake for 1 period
+	 * 32 - 254 = invalid
+	 * 255 = sleep at each ATIM
+	 */
+	__le16 adhoc_awake_period;
 } __packed;
 
 struct cmd_confirm_sleep {
@@ -882,12 +887,17 @@ struct cmd_ds_802_11_pa_cfg {
 
 
 struct cmd_ds_802_11_led_ctrl {
+	struct cmd_header hdr;
+
 	__le16 action;
 	__le16 numled;
 	u8 data[256];
 } __packed;
 
+/* Automatic Frequency Control */
 struct cmd_ds_802_11_afc {
+	struct cmd_header hdr;
+
 	__le16 afc_auto;
 	union {
 		struct {
@@ -910,6 +920,8 @@ struct cmd_ds_get_tsf {
 } __packed;
 
 struct cmd_ds_bt_access {
+	struct cmd_header hdr;
+
 	__le16 action;
 	__le32 id;
 	u8 addr1[ETH_ALEN];
@@ -917,6 +929,8 @@ struct cmd_ds_bt_access {
 } __packed;
 
 struct cmd_ds_fwt_access {
+	struct cmd_header hdr;
+
 	__le16 action;
 	__le32 id;
 	u8 valid;
@@ -955,34 +969,4 @@ struct cmd_ds_mesh_access {
 
 /* Number of stats counters returned by the firmware */
 #define MESH_STATS_NUM 8
-
-struct cmd_ds_command {
-	/* command header */
-	__le16 command;
-	__le16 size;
-	__le16 seqnum;
-	__le16 result;
-
-	/* command Body */
-	union {
-		struct cmd_ds_802_11_ps_mode psmode;
-		struct cmd_ds_802_11_monitor_mode monitor;
-		struct cmd_ds_802_11_rssi rssi;
-		struct cmd_ds_802_11_rssi_rsp rssirsp;
-		struct cmd_ds_mac_reg_access macreg;
-		struct cmd_ds_bbp_reg_access bbpreg;
-		struct cmd_ds_rf_reg_access rfreg;
-
-		struct cmd_ds_802_11d_domain_info domaininfo;
-		struct cmd_ds_802_11d_domain_info domaininforesp;
-
-		struct cmd_ds_802_11_tpc_cfg tpccfg;
-		struct cmd_ds_802_11_afc afc;
-		struct cmd_ds_802_11_led_ctrl ledgpio;
-
-		struct cmd_ds_bt_access bt;
-		struct cmd_ds_fwt_access fwt;
-		struct cmd_ds_802_11_beacon_control bcn_ctrl;
-	} params;
-} __packed;
 #endif
diff --git a/drivers/net/wireless/libertas/if_usb.c b/drivers/net/wireless/libertas/if_usb.c
index 3678e532874f..07ece9d26c63 100644
--- a/drivers/net/wireless/libertas/if_usb.c
+++ b/drivers/net/wireless/libertas/if_usb.c
@@ -433,7 +433,7 @@ static int if_usb_send_fw_pkt(struct if_usb_card *cardp)
 
 static int if_usb_reset_device(struct if_usb_card *cardp)
 {
-	struct cmd_ds_command *cmd = cardp->ep_out_buf + 4;
+	struct cmd_header *cmd = cardp->ep_out_buf + 4;
 	int ret;
 
 	lbs_deb_enter(LBS_DEB_USB);
@@ -441,7 +441,7 @@ static int if_usb_reset_device(struct if_usb_card *cardp)
 	*(__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_header));
+	cmd->size = cpu_to_le16(sizeof(cmd));
 	cmd->result = cpu_to_le16(0);
 	cmd->seqnum = cpu_to_le16(0x5a5a);
 	usb_tx_block(cardp, cardp->ep_out_buf, 4 + sizeof(struct cmd_header));
diff --git a/drivers/net/wireless/libertas/main.c b/drivers/net/wireless/libertas/main.c
index 2a0b590a93f1..258967144b96 100644
--- a/drivers/net/wireless/libertas/main.c
+++ b/drivers/net/wireless/libertas/main.c
@@ -157,12 +157,7 @@ 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);
+	/* FIXME: reset the card */
 
 	lbs_deb_leave(LBS_DEB_TX);
 }
@@ -507,12 +502,6 @@ static int lbs_thread(void *data)
 		if (!priv->dnld_sent && !priv->cur_cmd)
 			lbs_execute_next_command(priv);
 
-		/* Wake-up command waiters which can't sleep in
-		 * lbs_prepare_and_send_command
-		 */
-		if (!list_empty(&priv->cmdpendingq))
-			wake_up_all(&priv->cmd_pending);
-
 		spin_lock_irq(&priv->driver_lock);
 		if (!priv->dnld_sent && priv->tx_pending_len > 0) {
 			int ret = priv->hw_host_to_card(priv, MVMS_DAT,
@@ -538,7 +527,6 @@ static int lbs_thread(void *data)
 
 	del_timer(&priv->command_timer);
 	del_timer(&priv->auto_deepsleep_timer);
-	wake_up_all(&priv->cmd_pending);
 
 	lbs_deb_leave(LBS_DEB_THREAD);
 	return 0;
@@ -663,7 +651,6 @@ out:
 static void auto_deepsleep_timer_fn(unsigned long data)
 {
 	struct lbs_private *priv = (struct lbs_private *)data;
-	int ret;
 
 	lbs_deb_enter(LBS_DEB_CMD);
 
@@ -671,14 +658,15 @@ static void auto_deepsleep_timer_fn(unsigned long data)
 		priv->is_activity_detected = 0;
 	} else {
 		if (priv->is_auto_deep_sleep_enabled &&
-				(!priv->wakeup_dev_required) &&
-				(priv->connect_status != LBS_CONNECTED)) {
+		    (!priv->wakeup_dev_required) &&
+		    (priv->connect_status != LBS_CONNECTED)) {
+			struct cmd_header cmd;
+
 			lbs_deb_main("Entering auto deep sleep mode...\n");
-			ret = lbs_prepare_and_send_command(priv,
-					CMD_802_11_DEEP_SLEEP, 0,
-					0, 0, NULL);
-			if (ret)
-				lbs_pr_err("Enter Deep Sleep command failed\n");
+			memset(&cmd, 0, sizeof(cmd));
+			cmd.size = cpu_to_le16(sizeof(cmd));
+			lbs_cmd_async(priv, CMD_802_11_DEEP_SLEEP, &cmd,
+					sizeof(cmd));
 		}
 	}
 	mod_timer(&priv->auto_deepsleep_timer , jiffies +
@@ -746,7 +734,6 @@ static int lbs_init_adapter(struct lbs_private *priv)
 	INIT_LIST_HEAD(&priv->cmdpendingq);
 
 	spin_lock_init(&priv->driver_lock);
-	init_waitqueue_head(&priv->cmd_pending);
 
 	/* Allocate the command buffers */
 	if (lbs_allocate_cmd_buffer(priv)) {
@@ -902,7 +889,7 @@ void lbs_remove_card(struct lbs_private *priv)
 
 	if (priv->psmode == LBS802_11POWERMODEMAX_PSP) {
 		priv->psmode = LBS802_11POWERMODECAM;
-		lbs_ps_wakeup(priv, CMD_OPTION_WAITFORRSP);
+		lbs_set_ps_mode(priv, PS_MODE_ACTION_EXIT_PS, true);
 	}
 
 	if (priv->is_deep_sleep) {
@@ -1065,7 +1052,7 @@ static int __init lbs_init_module(void)
 	memset(&confirm_sleep, 0, sizeof(confirm_sleep));
 	confirm_sleep.hdr.command = cpu_to_le16(CMD_802_11_PS_MODE);
 	confirm_sleep.hdr.size = cpu_to_le16(sizeof(confirm_sleep));
-	confirm_sleep.action = cpu_to_le16(CMD_SUBCMD_SLEEP_CONFIRMED);
+	confirm_sleep.action = cpu_to_le16(PS_MODE_ACTION_SLEEP_CONFIRMED);
 	lbs_debugfs_init();
 	lbs_deb_leave(LBS_DEB_MAIN);
 	return 0;
diff --git a/drivers/net/wireless/libertas/mesh.c b/drivers/net/wireless/libertas/mesh.c
index bc5bc1384c35..194762ab0142 100644
--- a/drivers/net/wireless/libertas/mesh.c
+++ b/drivers/net/wireless/libertas/mesh.c
@@ -455,65 +455,189 @@ void lbs_mesh_set_txpd(struct lbs_private *priv,
  * Mesh command handling
  */
 
-int lbs_cmd_bt_access(struct cmd_ds_command *cmd,
-			       u16 cmd_action, void *pdata_buf)
+/**
+ *  @brief Add or delete Mesh Blinding Table entries
+ *
+ *  @param priv    	A pointer to struct lbs_private structure
+ *  @param add  	TRUE to add the entry, FALSE to delete it
+ *  @param addr1        Destination address to blind or unblind
+ *
+ *  @return 	   	0 on success, error on failure
+ */
+int lbs_mesh_bt_add_del(struct lbs_private *priv, bool add, u8 *addr1)
 {
-	struct cmd_ds_bt_access *bt_access = &cmd->params.bt;
-	lbs_deb_enter_args(LBS_DEB_CMD, "action %d", cmd_action);
+	struct cmd_ds_bt_access cmd;
+	int ret = 0;
 
-	cmd->command = cpu_to_le16(CMD_BT_ACCESS);
-	cmd->size = cpu_to_le16(sizeof(struct cmd_ds_bt_access) +
-		sizeof(struct cmd_header));
-	cmd->result = 0;
-	bt_access->action = cpu_to_le16(cmd_action);
+	lbs_deb_enter(LBS_DEB_CMD);
 
-	switch (cmd_action) {
-	case CMD_ACT_BT_ACCESS_ADD:
-		memcpy(bt_access->addr1, pdata_buf, 2 * ETH_ALEN);
+	BUG_ON(addr1 == NULL);
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
+	memcpy(cmd.addr1, addr1, ETH_ALEN);
+	if (add) {
+		cmd.action = cpu_to_le16(CMD_ACT_BT_ACCESS_ADD);
 		lbs_deb_hex(LBS_DEB_MESH, "BT_ADD: blinded MAC addr",
-			bt_access->addr1, 6);
-		break;
-	case CMD_ACT_BT_ACCESS_DEL:
-		memcpy(bt_access->addr1, pdata_buf, 1 * ETH_ALEN);
+			addr1, ETH_ALEN);
+	} else {
+		cmd.action = cpu_to_le16(CMD_ACT_BT_ACCESS_DEL);
 		lbs_deb_hex(LBS_DEB_MESH, "BT_DEL: blinded MAC addr",
-			bt_access->addr1, 6);
-		break;
-	case CMD_ACT_BT_ACCESS_LIST:
-		bt_access->id = cpu_to_le32(*(u32 *) pdata_buf);
-		break;
-	case CMD_ACT_BT_ACCESS_RESET:
-		break;
-	case CMD_ACT_BT_ACCESS_SET_INVERT:
-		bt_access->id = cpu_to_le32(*(u32 *) pdata_buf);
-		break;
-	case CMD_ACT_BT_ACCESS_GET_INVERT:
-		break;
-	default:
-		break;
+			addr1, ETH_ALEN);
 	}
-	lbs_deb_leave(LBS_DEB_CMD);
-	return 0;
+
+	ret = lbs_cmd_with_response(priv, CMD_BT_ACCESS, &cmd);
+
+	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
+	return ret;
 }
 
-int lbs_cmd_fwt_access(struct cmd_ds_command *cmd,
-			       u16 cmd_action, void *pdata_buf)
+/**
+ *  @brief Reset/clear the mesh blinding table
+ *
+ *  @param priv    	A pointer to struct lbs_private structure
+ *
+ *  @return 	   	0 on success, error on failure
+ */
+int lbs_mesh_bt_reset(struct lbs_private *priv)
 {
-	struct cmd_ds_fwt_access *fwt_access = &cmd->params.fwt;
-	lbs_deb_enter_args(LBS_DEB_CMD, "action %d", cmd_action);
+	struct cmd_ds_bt_access cmd;
+	int ret = 0;
 
-	cmd->command = cpu_to_le16(CMD_FWT_ACCESS);
-	cmd->size = cpu_to_le16(sizeof(struct cmd_ds_fwt_access) +
-		sizeof(struct cmd_header));
-	cmd->result = 0;
+	lbs_deb_enter(LBS_DEB_CMD);
 
-	if (pdata_buf)
-		memcpy(fwt_access, pdata_buf, sizeof(*fwt_access));
-	else
-		memset(fwt_access, 0, sizeof(*fwt_access));
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
+	cmd.action = cpu_to_le16(CMD_ACT_BT_ACCESS_RESET);
 
-	fwt_access->action = cpu_to_le16(cmd_action);
+	ret = lbs_cmd_with_response(priv, CMD_BT_ACCESS, &cmd);
 
-	lbs_deb_leave(LBS_DEB_CMD);
+	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
+	return ret;
+}
+
+/**
+ *  @brief Gets the inverted status of the mesh blinding table
+ *
+ *  Normally the firmware "blinds" or ignores traffic from mesh nodes in the
+ *  table, but an inverted table allows *only* traffic from nodes listed in
+ *  the table.
+ *
+ *  @param priv    	A pointer to struct lbs_private structure
+ *  @param invert  	On success, TRUE if the blinding table is inverted,
+ *                        FALSE if it is not inverted
+ *
+ *  @return 	   	0 on success, error on failure
+ */
+int lbs_mesh_bt_get_inverted(struct lbs_private *priv, bool *inverted)
+{
+	struct cmd_ds_bt_access cmd;
+	int ret = 0;
+
+	lbs_deb_enter(LBS_DEB_CMD);
+
+	BUG_ON(inverted == NULL);
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
+	cmd.action = cpu_to_le16(CMD_ACT_BT_ACCESS_GET_INVERT);
+
+	ret = lbs_cmd_with_response(priv, CMD_BT_ACCESS, &cmd);
+	if (ret == 0)
+		*inverted = !!cmd.id;
+
+	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
+	return ret;
+}
+
+/**
+ *  @brief Sets the inverted status of the mesh blinding table
+ *
+ *  Normally the firmware "blinds" or ignores traffic from mesh nodes in the
+ *  table, but an inverted table allows *only* traffic from nodes listed in
+ *  the table.
+ *
+ *  @param priv    	A pointer to struct lbs_private structure
+ *  @param invert  	TRUE to invert the blinding table (only traffic from
+ *                         listed nodes allowed), FALSE to return it
+ *                         to normal state (listed nodes ignored)
+ *
+ *  @return 	   	0 on success, error on failure
+ */
+int lbs_mesh_bt_set_inverted(struct lbs_private *priv, bool inverted)
+{
+	struct cmd_ds_bt_access cmd;
+	int ret = 0;
+
+	lbs_deb_enter(LBS_DEB_CMD);
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
+	cmd.action = cpu_to_le16(CMD_ACT_BT_ACCESS_SET_INVERT);
+	cmd.id = !!inverted;
+
+	ret = lbs_cmd_with_response(priv, CMD_BT_ACCESS, &cmd);
+
+	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
+	return ret;
+}
+
+/**
+ *  @brief List an entry in the mesh blinding table
+ *
+ *  @param priv    	A pointer to struct lbs_private structure
+ *  @param id		The ID of the entry to list
+ *  @param addr1	MAC address associated with the table entry
+ *
+ *  @return 	   	0 on success, error on failure
+ */
+int lbs_mesh_bt_get_entry(struct lbs_private *priv, u32 id, u8 *addr1)
+{
+	struct cmd_ds_bt_access cmd;
+	int ret = 0;
+
+	lbs_deb_enter(LBS_DEB_CMD);
+
+	BUG_ON(addr1 == NULL);
+
+	memset(&cmd, 0, sizeof(cmd));
+	cmd.hdr.size = cpu_to_le16(sizeof(cmd));
+	cmd.action = cpu_to_le16(CMD_ACT_BT_ACCESS_SET_INVERT);
+	cmd.id = cpu_to_le32(id);
+
+	ret = lbs_cmd_with_response(priv, CMD_BT_ACCESS, &cmd);
+	if (ret == 0)
+		memcpy(addr1, cmd.addr1, sizeof(cmd.addr1));
+
+	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
+	return ret;
+}
+
+/**
+ *  @brief Access the mesh forwarding table
+ *
+ *  @param priv    	A pointer to struct lbs_private structure
+ *  @param cmd_action	The forwarding table action to perform
+ *  @param cmd		The pre-filled FWT_ACCESS command
+ *
+ *  @return 	   	0 on success and 'cmd' will be filled with the
+ *                        firmware's response
+ */
+int lbs_cmd_fwt_access(struct lbs_private *priv, u16 cmd_action,
+			struct cmd_ds_fwt_access *cmd)
+{
+	int ret;
+
+	lbs_deb_enter_args(LBS_DEB_CMD, "action %d", cmd_action);
+
+	cmd->hdr.command = cpu_to_le16(CMD_FWT_ACCESS);
+	cmd->hdr.size = cpu_to_le16(sizeof(struct cmd_ds_fwt_access));
+	cmd->hdr.result = 0;
+	cmd->action = cpu_to_le16(cmd_action);
+
+	ret = lbs_cmd_with_response(priv, CMD_FWT_ACCESS, cmd);
+
+	lbs_deb_leave_args(LBS_DEB_CMD, "ret %d", ret);
 	return 0;
 }
 
diff --git a/drivers/net/wireless/libertas/mesh.h b/drivers/net/wireless/libertas/mesh.h
index 84ea2481ff20..afb2e8dead3f 100644
--- a/drivers/net/wireless/libertas/mesh.h
+++ b/drivers/net/wireless/libertas/mesh.h
@@ -8,6 +8,7 @@
 #include <net/iw_handler.h>
 #include <net/lib80211.h>
 
+#include "host.h"
 
 #ifdef CONFIG_LIBERTAS_MESH
 
@@ -51,10 +52,15 @@ struct cmd_ds_command;
 struct cmd_ds_mesh_access;
 struct cmd_ds_mesh_config;
 
-int lbs_cmd_bt_access(struct cmd_ds_command *cmd,
-	u16 cmd_action, void *pdata_buf);
-int lbs_cmd_fwt_access(struct cmd_ds_command *cmd,
-	u16 cmd_action, void *pdata_buf);
+int lbs_mesh_bt_add_del(struct lbs_private *priv, bool add, u8 *addr1);
+int lbs_mesh_bt_reset(struct lbs_private *priv);
+int lbs_mesh_bt_get_inverted(struct lbs_private *priv, bool *inverted);
+int lbs_mesh_bt_set_inverted(struct lbs_private *priv, bool inverted);
+int lbs_mesh_bt_get_entry(struct lbs_private *priv, u32 id, u8 *addr1);
+
+int lbs_cmd_fwt_access(struct lbs_private *priv, u16 cmd_action,
+			struct cmd_ds_fwt_access *cmd);
+
 int lbs_mesh_access(struct lbs_private *priv, uint16_t cmd_action,
 		    struct cmd_ds_mesh_access *cmd);
 int lbs_mesh_config_send(struct lbs_private *priv,
diff --git a/drivers/net/wireless/libertas/tx.c b/drivers/net/wireless/libertas/tx.c
index 411a3bbf035e..8000ca6165d0 100644
--- a/drivers/net/wireless/libertas/tx.c
+++ b/drivers/net/wireless/libertas/tx.c
@@ -180,7 +180,7 @@ void lbs_send_tx_feedback(struct lbs_private *priv, u32 try_count)
 {
 	struct tx_radiotap_hdr *radiotap_hdr;
 
-	if (!priv->wdev->iftype == NL80211_IFTYPE_MONITOR ||
+	if (priv->wdev->iftype != NL80211_IFTYPE_MONITOR ||
 	    priv->currenttxskb == NULL)
 		return;