4 files changed, 55 insertions, 8 deletions

diff --git a/drivers/net/wireless/libertas/assoc.c b/drivers/net/wireless/libertas/assoc.c
index f67efa0815fe..afd5617dd92b 100644
--- a/drivers/net/wireless/libertas/assoc.c
+++ b/drivers/net/wireless/libertas/assoc.c
@@ -323,6 +323,8 @@ static int assoc_helper_secinfo(wlan_private *priv,
 {
 	wlan_adapter *adapter = priv->adapter;
 	int ret = 0;
+	u32 do_wpa;
+	u32 rsn = 0;
 
 	lbs_deb_enter(LBS_DEB_ASSOC);
 
@@ -333,12 +335,34 @@ static int assoc_helper_secinfo(wlan_private *priv,
 	if (ret)
 		goto out;
 
-	/* enable/disable RSN */
+	/* If RSN is already enabled, don't try to enable it again, since
+	 * ENABLE_RSN resets internal state machines and will clobber the
+	 * 4-way WPA handshake.
+	 */
+
+	/* Get RSN enabled/disabled */
 	ret = libertas_prepare_and_send_command(priv,
 				    cmd_802_11_enable_rsn,
 				    cmd_act_set,
 				    cmd_option_waitforrsp,
-				    0, assoc_req);
+				    0, &rsn);
+	if (ret) {
+		lbs_deb_assoc("Failed to get RSN status: %d", ret);
+		goto out;
+	}
+
+	/* Don't re-enable RSN if it's already enabled */
+	do_wpa = (assoc_req->secinfo.WPAenabled || assoc_req->secinfo.WPA2enabled);
+	if (do_wpa == rsn)
+		goto out;
+
+	/* Set RSN enabled/disabled */
+	rsn = do_wpa;
+	ret = libertas_prepare_and_send_command(priv,
+				    cmd_802_11_enable_rsn,
+				    cmd_act_set,
+				    cmd_option_waitforrsp,
+				    0, &rsn);
 
 out:
 	lbs_deb_leave_args(LBS_DEB_ASSOC, "ret %d", ret);
diff --git a/drivers/net/wireless/libertas/cmd.c b/drivers/net/wireless/libertas/cmd.c
index 124e029f1bf4..13f6528abb00 100644
--- a/drivers/net/wireless/libertas/cmd.c
+++ b/drivers/net/wireless/libertas/cmd.c
@@ -228,17 +228,19 @@ static int wlan_cmd_802_11_enable_rsn(wlan_private * priv,
 				      void * pdata_buf)
 {
 	struct cmd_ds_802_11_enable_rsn *penableRSN = &cmd->params.enbrsn;
-	struct assoc_request * assoc_req = pdata_buf;
+	u32 * enable = pdata_buf;
 
 	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);
-	if (assoc_req->secinfo.WPAenabled || assoc_req->secinfo.WPA2enabled) {
-		penableRSN->enable = cpu_to_le16(cmd_enable_rsn);
-	} else {
-		penableRSN->enable = cpu_to_le16(cmd_disable_rsn);
+
+	if (cmd_action == cmd_act_set) {
+		if (*enable)
+			penableRSN->enable = cpu_to_le16(cmd_enable_rsn);
+		else
+			penableRSN->enable = cpu_to_le16(cmd_enable_rsn);
 	}
 
 	lbs_deb_leave(LBS_DEB_CMD);
diff --git a/drivers/net/wireless/libertas/cmdresp.c b/drivers/net/wireless/libertas/cmdresp.c
index 0c3b9a583d83..6ac0d4752fa4 100644
--- a/drivers/net/wireless/libertas/cmdresp.c
+++ b/drivers/net/wireless/libertas/cmdresp.c
@@ -537,6 +537,24 @@ static int wlan_ret_get_log(wlan_private * priv,
 	return 0;
 }
 
+static int libertas_ret_802_11_enable_rsn(wlan_private * priv,
+                                          struct cmd_ds_command *resp)
+{
+	struct cmd_ds_802_11_enable_rsn *enable_rsn = &resp->params.enbrsn;
+	wlan_adapter *adapter = priv->adapter;
+	u32 * pdata_buf = adapter->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_enter(LBS_DEB_CMD);
+	return 0;
+}
+
 static inline int handle_cmd_response(u16 respcmd,
 				      struct cmd_ds_command *resp,
 				      wlan_private *priv)
@@ -610,7 +628,10 @@ static inline int handle_cmd_response(u16 respcmd,
 	case cmd_ret_802_11_authenticate:
 	case cmd_ret_802_11_radio_control:
 	case cmd_ret_802_11_beacon_stop:
+		break;
+
 	case cmd_ret_802_11_enable_rsn:
+		ret = libertas_ret_802_11_enable_rsn(priv, resp);
 		break;
 
 	case cmd_ret_802_11_data_rate:
diff --git a/drivers/net/wireless/libertas/hostcmd.h b/drivers/net/wireless/libertas/hostcmd.h
index 3acf93988125..09b898f6719c 100644
--- a/drivers/net/wireless/libertas/hostcmd.h
+++ b/drivers/net/wireless/libertas/hostcmd.h
@@ -503,7 +503,7 @@ struct cmd_ds_802_11_ad_hoc_join {
 struct cmd_ds_802_11_enable_rsn {
 	__le16 action;
 	__le16 enable;
-};
+} __attribute__ ((packed));
 
 struct MrvlIEtype_keyParamSet {
 	/* type ID */