From 20f5a4c24c69fbbae0362e073369254db75c7f0d Mon Sep 17 00:00:00 2001
From: Jouni Malinen <j@w1.fi>
Date: Sun, 18 Sep 2011 21:26:55 +0300
Subject: [PATCH] nl80211: Work around EALREADY from connect command

cfg80211 does not currently allow NL80211_CMD_CONNECT when there
is already an association. This can result in wpa_supplicant being
unable to request a connection if the interface is already in
connected state. Work around this by requesting disconnection if
the connect command fails with EALREADY error. This allows
wpa_supplicant to recover from the unexpected state and complete
the connect request on next attempt.
---
 src/drivers/driver_nl80211.c | 107 +++++++++++++++++++++++++++++------
 1 file changed, 91 insertions(+), 16 deletions(-)

diff --git a/src/drivers/driver_nl80211.c b/src/drivers/driver_nl80211.c
index 19893255c..625d7f220 100644
--- a/src/drivers/driver_nl80211.c
+++ b/src/drivers/driver_nl80211.c
@@ -248,6 +248,7 @@ struct nl80211_bss_info_arg {
 	struct wpa_driver_nl80211_data *drv;
 	struct wpa_scan_results *res;
 	unsigned int assoc_freq;
+	u8 assoc_bssid[ETH_ALEN];
 };
 
 static int bss_info_handler(struct nl_msg *msg, void *arg);
@@ -757,6 +758,29 @@ static void mlme_event_connect(struct wpa_driver_nl80211_data *drv,
 }
 
 
+static void mlme_event_disconnect(struct wpa_driver_nl80211_data *drv,
+				  struct nlattr *reason, struct nlattr *addr)
+{
+	union wpa_event_data data;
+
+	if (drv->capa.flags & WPA_DRIVER_FLAGS_SME) {
+		/*
+		 * Avoid reporting two disassociation events that could
+		 * confuse the core code.
+		 */
+		wpa_printf(MSG_DEBUG, "nl80211: Ignore disconnect "
+			   "event when using userspace SME");
+		return;
+	}
+
+	drv->associated = 0;
+	os_memset(&data, 0, sizeof(data));
+	if (reason)
+		data.disassoc_info.reason_code = nla_get_u16(reason);
+	wpa_supplicant_event(drv->ctx, EVENT_DISASSOC, &data);
+}
+
+
 static void mlme_timeout_event(struct wpa_driver_nl80211_data *drv,
 			       enum nl80211_commands cmd, struct nlattr *addr)
 {
@@ -1452,7 +1476,6 @@ static int process_event(struct nl_msg *msg, void *arg)
 	struct wpa_driver_nl80211_data *drv = arg;
 	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 	struct nlattr *tb[NL80211_ATTR_MAX + 1];
-	union wpa_event_data data;
 
 	nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
 		  genlmsg_attrlen(gnlh, 0), NULL);
@@ -1518,21 +1541,8 @@ static int process_event(struct nl_msg *msg, void *arg)
 				   tb[NL80211_ATTR_RESP_IE]);
 		break;
 	case NL80211_CMD_DISCONNECT:
-		if (drv->capa.flags & WPA_DRIVER_FLAGS_SME) {
-			/*
-			 * Avoid reporting two disassociation events that could
-			 * confuse the core code.
-			 */
-			wpa_printf(MSG_DEBUG, "nl80211: Ignore disconnect "
-				   "event when using userspace SME");
-			break;
-		}
-		drv->associated = 0;
-		os_memset(&data, 0, sizeof(data));
-		if (tb[NL80211_ATTR_REASON_CODE])
-			data.disassoc_info.reason_code =
-				nla_get_u16(tb[NL80211_ATTR_REASON_CODE]);
-		wpa_supplicant_event(drv->ctx, EVENT_DISASSOC, &data);
+		mlme_event_disconnect(drv, tb[NL80211_ATTR_REASON_CODE],
+				      tb[NL80211_ATTR_MAC]);
 		break;
 	case NL80211_CMD_MICHAEL_MIC_FAILURE:
 		mlme_event_michael_mic_failure(drv, tb);
@@ -2648,6 +2658,13 @@ static int bss_info_handler(struct nl_msg *msg, void *arg)
 			wpa_printf(MSG_DEBUG, "nl80211: Associated on %u MHz",
 				   _arg->assoc_freq);
 		}
+		if (status == NL80211_BSS_STATUS_ASSOCIATED &&
+		    bss[NL80211_BSS_BSSID]) {
+			os_memcpy(_arg->assoc_bssid,
+				  nla_data(bss[NL80211_BSS_BSSID]), ETH_ALEN);
+			wpa_printf(MSG_DEBUG, "nl80211: Associated with "
+				   MACSTR, MAC2STR(_arg->assoc_bssid));
+		}
 	}
 	if (!res)
 		return NL_SKIP;
@@ -4978,6 +4995,56 @@ nla_put_failure:
 }
 
 
+static unsigned int nl80211_get_assoc_bssid(struct wpa_driver_nl80211_data *drv,
+					    u8 *bssid)
+{
+	struct nl_msg *msg;
+	int ret;
+	struct nl80211_bss_info_arg arg;
+
+	os_memset(&arg, 0, sizeof(arg));
+	msg = nlmsg_alloc();
+	if (!msg)
+		goto nla_put_failure;
+
+	genlmsg_put(msg, 0, 0, genl_family_get_id(drv->nl80211), 0, NLM_F_DUMP,
+		    NL80211_CMD_GET_SCAN, 0);
+	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, drv->ifindex);
+
+	arg.drv = drv;
+	ret = send_and_recv_msgs(drv, msg, bss_info_handler, &arg);
+	msg = NULL;
+	if (ret == 0) {
+		if (is_zero_ether_addr(arg.assoc_bssid))
+			return -ENOTCONN;
+		os_memcpy(bssid, arg.assoc_bssid, ETH_ALEN);
+		return 0;
+	}
+	wpa_printf(MSG_DEBUG, "nl80211: Scan result fetch failed: ret=%d "
+		   "(%s)", ret, strerror(-ret));
+nla_put_failure:
+	nlmsg_free(msg);
+	return drv->assoc_freq;
+}
+
+
+static int nl80211_disconnect(struct wpa_driver_nl80211_data *drv,
+			      const u8 *bssid)
+{
+	u8 addr[ETH_ALEN];
+
+	if (bssid == NULL) {
+		int res = nl80211_get_assoc_bssid(drv, addr);
+		if (res)
+			return res;
+		bssid = addr;
+	}
+
+	return wpa_driver_nl80211_disconnect(drv, bssid,
+					     WLAN_REASON_PREV_AUTH_NOT_VALID);
+}
+
+
 static int wpa_driver_nl80211_connect(
 	struct wpa_driver_nl80211_data *drv,
 	struct wpa_driver_associate_params *params)
@@ -5127,6 +5194,14 @@ skip_auth_type:
 	if (ret) {
 		wpa_printf(MSG_DEBUG, "nl80211: MLME connect failed: ret=%d "
 			   "(%s)", ret, strerror(-ret));
+		/*
+		 * cfg80211 does not currently accept new connection if we are
+		 * already connected. As a workaround, force disconnection and
+		 * try again once the driver indicates it completed
+		 * disconnection.
+		 */
+		if (ret == -EALREADY)
+			nl80211_disconnect(drv, params->bssid);
 		goto nla_put_failure;
 	}
 	ret = 0;