wl12xx: fix testmode test/interrogate commands

fix several issues in testmode test/interrogate commands:
1. check the driver state is not OFF.
2. wakeup the chip from elp (if needed)
3. fix memory leak in wl1271_tm_cmd_interrogate()

Signed-off-by: Eliad Peller <eliad@wizery.com>
Signed-off-by: Luciano Coelho <coelho@ti.com>

drivers/net/wireless/wl12xx/testmode.c

@@ -29,6 +29,7 @@
 #include "debug.h"
 #include "acx.h"
 #include "reg.h"
+#include "ps.h"
 
 #define WL1271_TM_MAX_DATA_LENGTH 1024
 
@@ -88,31 +89,47 @@ static int wl1271_tm_cmd_test(struct wl1271 *wl, struct nlattr *tb[])
 		return -EMSGSIZE;
 
 	mutex_lock(&wl->mutex);
-	ret = wl1271_cmd_test(wl, buf, buf_len, answer);
-	mutex_unlock(&wl->mutex);
 
+	if (wl->state == WL1271_STATE_OFF) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	ret = wl1271_ps_elp_wakeup(wl);
+	if (ret < 0)
+		goto out;
+
+	ret = wl1271_cmd_test(wl, buf, buf_len, answer);
 	if (ret < 0) {
 		wl1271_warning("testmode cmd test failed: %d", ret);
-		return ret;
+		goto out_sleep;
 	}
 
 	if (answer) {
 		len = nla_total_size(buf_len);
 		skb = cfg80211_testmode_alloc_reply_skb(wl->hw->wiphy, len);
-		if (!skb)
+		if (!skb) {
-			return -ENOMEM;
+			ret = -ENOMEM;
+			goto out_sleep;
+		}
 
 		NLA_PUT(skb, WL1271_TM_ATTR_DATA, buf_len, buf);
 		ret = cfg80211_testmode_reply(skb);
 		if (ret < 0)
-			return ret;
+			goto out_sleep;
 	}
 
-	return 0;
+out_sleep:
+	wl1271_ps_elp_sleep(wl);
+out:
+	mutex_unlock(&wl->mutex);
+
+	return ret;
 
 nla_put_failure:
 	kfree_skb(skb);
-	return -EMSGSIZE;
+	ret = -EMSGSIZE;
+	goto out_sleep;
 }
 
 static int wl1271_tm_cmd_interrogate(struct wl1271 *wl, struct nlattr *tb[])
@@ -129,33 +146,50 @@ static int wl1271_tm_cmd_interrogate(struct wl1271 *wl, struct nlattr *tb[])
 
 	ie_id = nla_get_u8(tb[WL1271_TM_ATTR_IE_ID]);
 
-	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
-	if (!cmd)
-		return -ENOMEM;
-
 	mutex_lock(&wl->mutex);
-	ret = wl1271_cmd_interrogate(wl, ie_id, cmd, sizeof(*cmd));
-	mutex_unlock(&wl->mutex);
 
+	if (wl->state == WL1271_STATE_OFF) {
+		ret = -EINVAL;
+		goto out;
+	}
+
+	ret = wl1271_ps_elp_wakeup(wl);
+	if (ret < 0)
+		goto out;
+
+	cmd = kzalloc(sizeof(*cmd), GFP_KERNEL);
+	if (!cmd) {
+		ret = -ENOMEM;
+		goto out_sleep;
+	}
+
+	ret = wl1271_cmd_interrogate(wl, ie_id, cmd, sizeof(*cmd));
 	if (ret < 0) {
 		wl1271_warning("testmode cmd interrogate failed: %d", ret);
-		kfree(cmd);
+		goto out_free;
-		return ret;
 	}
 
 	skb = cfg80211_testmode_alloc_reply_skb(wl->hw->wiphy, sizeof(*cmd));
 	if (!skb) {
-		kfree(cmd);
+		ret = -ENOMEM;
-		return -ENOMEM;
+		goto out_free;
 	}
 
 	NLA_PUT(skb, WL1271_TM_ATTR_DATA, sizeof(*cmd), cmd);
 
-	return 0;
+out_free:
+	kfree(cmd);
+out_sleep:
+	wl1271_ps_elp_sleep(wl);
+out:
+	mutex_unlock(&wl->mutex);
+
+	return ret;
 
 nla_put_failure:
 	kfree_skb(skb);
-	return -EMSGSIZE;
+	ret = -EMSGSIZE;
+	goto out_free;
 }
 
 static int wl1271_tm_cmd_configure(struct wl1271 *wl, struct nlattr *tb[])