1 files changed, 71 insertions, 41 deletions

diff --git a/drivers/net/wireless/p54/main.c b/drivers/net/wireless/p54/main.c
index f9b4f6a238ea..955f6d7ec16a 100644
--- a/drivers/net/wireless/p54/main.c
+++ b/drivers/net/wireless/p54/main.c
@@ -65,51 +65,64 @@ static int p54_set_tim(struct ieee80211_hw *dev, struct ieee80211_sta *sta,
 	return p54_update_beacon_tim(priv, sta->aid, set);
 }
 
-static int p54_beacon_format_ie_tim(struct sk_buff *skb)
+u8 *p54_find_ie(struct sk_buff *skb, u8 ie)
 {
-	/*
-	 * the good excuse for this mess is ... the firmware.
-	 * The dummy TIM MUST be at the end of the beacon frame,
-	 * because it'll be overwritten!
-	 */
-
 	struct ieee80211_mgmt *mgmt = (void *)skb->data;
 	u8 *pos, *end;
 
 	if (skb->len <= sizeof(mgmt))
-		return -EINVAL;
+		return NULL;
 
 	pos = (u8 *)mgmt->u.beacon.variable;
 	end = skb->data + skb->len;
 	while (pos < end) {
 		if (pos + 2 + pos[1] > end)
-			return -EINVAL;
+			return NULL;
+
+		if (pos[0] == ie)
+			return pos;
+
+		pos += 2 + pos[1];
+	}
+	return NULL;
+}
+
+static int p54_beacon_format_ie_tim(struct sk_buff *skb)
+{
+	/*
+	 * the good excuse for this mess is ... the firmware.
+	 * The dummy TIM MUST be at the end of the beacon frame,
+	 * because it'll be overwritten!
+	 */
+	u8 *tim;
+	u8 dtim_len;
+	u8 dtim_period;
+	u8 *next;
 
-		if (pos[0] == WLAN_EID_TIM) {
-			u8 dtim_len = pos[1];
-			u8 dtim_period = pos[3];
-			u8 *next = pos + 2 + dtim_len;
+	tim = p54_find_ie(skb, WLAN_EID_TIM);
+	if (!tim)
+		return 0;
 
-			if (dtim_len < 3)
-				return -EINVAL;
+	dtim_len = tim[1];
+	dtim_period = tim[3];
+	next = tim + 2 + dtim_len;
 
-			memmove(pos, next, end - next);
+	if (dtim_len < 3)
+		return -EINVAL;
 
-			if (dtim_len > 3)
-				skb_trim(skb, skb->len - (dtim_len - 3));
+	memmove(tim, next, skb_tail_pointer(skb) - next);
+	tim = skb_tail_pointer(skb) - (dtim_len + 2);
 
-			pos = end - (dtim_len + 2);
+	/* add the dummy at the end */
+	tim[0] = WLAN_EID_TIM;
+	tim[1] = 3;
+	tim[2] = 0;
+	tim[3] = dtim_period;
+	tim[4] = 0;
+
+	if (dtim_len > 3)
+		skb_trim(skb, skb->len - (dtim_len - 3));
 
-			/* add the dummy at the end */
-			pos[0] = WLAN_EID_TIM;
-			pos[1] = 3;
-			pos[2] = 0;
-			pos[3] = dtim_period;
-			pos[4] = 0;
-			return 0;
-		}
-		pos += 2 + pos[1];
-	}
 	return 0;
 }
 
@@ -117,7 +130,6 @@ static int p54_beacon_update(struct p54_common *priv,
 			struct ieee80211_vif *vif)
 {
 	struct sk_buff *beacon;
-	__le32 old_beacon_req_id;
 	int ret;
 
 	beacon = ieee80211_beacon_get(priv->hw, vif);
@@ -127,15 +139,16 @@ static int p54_beacon_update(struct p54_common *priv,
 	if (ret)
 		return ret;
 
-	old_beacon_req_id = priv->beacon_req_id;
-	priv->beacon_req_id = GET_REQ_ID(beacon);
-
-	ret = p54_tx_80211(priv->hw, beacon);
-	if (ret) {
-		priv->beacon_req_id = old_beacon_req_id;
-		return -ENOSPC;
-	}
-
+	/*
+	 * During operation, the firmware takes care of beaconing.
+	 * The driver only needs to upload a new beacon template, once
+	 * the template was changed by the stack or userspace.
+	 *
+	 * LMAC API 3.2.2 also specifies that the driver does not need
+	 * to cancel the old beacon template by hand, instead the firmware
+	 * will release the previous one through the feedback mechanism.
+	 */
+	WARN_ON(p54_tx_80211(priv->hw, beacon));
 	priv->tsf_high32 = 0;
 	priv->tsf_low32 = 0;
 
@@ -240,9 +253,14 @@ static void p54_remove_interface(struct ieee80211_hw *dev,
 
 	mutex_lock(&priv->conf_mutex);
 	priv->vif = NULL;
-	if (priv->beacon_req_id) {
+
+	/*
+	 * LMAC API 3.2.2 states that any active beacon template must be
+	 * canceled by the driver before attempting a mode transition.
+	 */
+	if (le32_to_cpu(priv->beacon_req_id) != 0) {
 		p54_tx_cancel(priv, priv->beacon_req_id);
-		priv->beacon_req_id = cpu_to_le32(0);
+		wait_for_completion_interruptible_timeout(&priv->beacon_comp, HZ);
 	}
 	priv->mode = NL80211_IFTYPE_MONITOR;
 	memset(priv->mac_addr, 0, ETH_ALEN);
@@ -384,6 +402,9 @@ static void p54_bss_info_changed(struct ieee80211_hw *dev,
 			priv->wakeup_timer = info->beacon_int *
 					     info->dtim_period * 5;
 			p54_setup_mac(priv);
+		} else {
+			priv->wakeup_timer = 500;
+			priv->aid = 0;
 		}
 	}
 
@@ -517,6 +538,9 @@ struct ieee80211_hw *p54_init_common(size_t priv_data_len)
 	skb_queue_head_init(&priv->tx_pending);
 	dev->flags = IEEE80211_HW_RX_INCLUDES_FCS |
 		     IEEE80211_HW_SIGNAL_DBM |
+		     IEEE80211_HW_SUPPORTS_PS |
+		     IEEE80211_HW_PS_NULLFUNC_STACK |
+		     IEEE80211_HW_BEACON_FILTER |
 		     IEEE80211_HW_NOISE_DBM;
 
 	dev->wiphy->interface_modes = BIT(NL80211_IFTYPE_STATION) |
@@ -525,6 +549,7 @@ struct ieee80211_hw *p54_init_common(size_t priv_data_len)
 				      BIT(NL80211_IFTYPE_MESH_POINT);
 
 	dev->channel_change_time = 1000;	/* TODO: find actual value */
+	priv->beacon_req_id = cpu_to_le32(0);
 	priv->tx_stats[P54_QUEUE_BEACON].limit = 1;
 	priv->tx_stats[P54_QUEUE_FWSCAN].limit = 1;
 	priv->tx_stats[P54_QUEUE_MGMT].limit = 3;
@@ -548,6 +573,7 @@ struct ieee80211_hw *p54_init_common(size_t priv_data_len)
 	mutex_init(&priv->conf_mutex);
 	mutex_init(&priv->eeprom_mutex);
 	init_completion(&priv->eeprom_comp);
+	init_completion(&priv->beacon_comp);
 	INIT_DELAYED_WORK(&priv->work, p54_work);
 
 	return dev;
@@ -579,6 +605,10 @@ EXPORT_SYMBOL_GPL(p54_register_common);
 void p54_free_common(struct ieee80211_hw *dev)
 {
 	struct p54_common *priv = dev->priv;
+	unsigned int i;
+
+	for (i = 0; i < IEEE80211_NUM_BANDS; i++)
+		kfree(priv->band_table[i]);
 
 	kfree(priv->iq_autocal);
 	kfree(priv->output_limit);