cw1200: Fix for buffered multicasts in AP-PS mode.

Refactoring of multicast handling:
 * When multicast is coming, driver sets aid0 bit in TIM IE and puts
   multicas in the queue with "AFTER_DTIM" link id.
 * When WSM indicates DTIM beacon, driver verifies if aid0 indicating
   beacon was sent and sends buffered multicasts.
 * Driver clears aid0 bit in the TIM IE when no more multicasts are
   available.

 + A stilistic change: 1 << shift is replaced by BIT(shift)

Change-Id: I15b134bf847913a907f00293ae99d7a71bbc7343
Signed-off-by: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/27306
Reviewed-by: Bartosz MARKOWSKI <bartosz.markowski@tieto.com>
Tested-by: Bartosz MARKOWSKI <bartosz.markowski@tieto.com>
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/33488
Reviewed-by: Philippe LANGLAIS <philippe.langlais@stericsson.com>

8 files changed, 105 insertions, 51 deletions

diff --git a/drivers/staging/cw1200/ap.c b/drivers/staging/cw1200/ap.c
index c6e5b78ab66..fc23ff74100 100755
--- a/drivers/staging/cw1200/ap.c
+++ b/drivers/staging/cw1200/ap.c
@@ -51,7 +51,7 @@ int cw1200_sta_add(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	memcpy(map_link.mac_addr, sta->addr, ETH_ALEN);
 	if (!WARN_ON(wsm_map_link(priv, &map_link))) {
 		sta_priv->link_id = map_link.link_id;
-		priv->link_id_map |= 1 << map_link.link_id;
+		priv->link_id_map |= BIT(map_link.link_id);
 		ap_printk(KERN_DEBUG "[AP] STA added, link_id: %d\n",
 			map_link.link_id);
 	}
@@ -73,7 +73,7 @@ int cw1200_sta_remove(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 		ap_printk(KERN_DEBUG "[AP] STA removed, link_id: %d\n",
 			sta_priv->link_id);
 		reset.link_id = sta_priv->link_id;
-		priv->link_id_map &= ~(1 << sta_priv->link_id);
+		priv->link_id_map &= ~BIT(sta_priv->link_id);
 		sta_priv->link_id = 0;
 		WARN_ON(wsm_reset(priv, &reset));
 	}
@@ -87,7 +87,7 @@ void cw1200_sta_notify(struct ieee80211_hw *dev, struct ieee80211_vif *vif,
 	struct cw1200_common *priv = dev->priv;
 	struct cw1200_sta_priv *sta_priv =
 		(struct cw1200_sta_priv *)&sta->drv_priv;
-	u32 bit = 1 << sta_priv->link_id;
+	u32 bit = BIT(sta_priv->link_id);
 
 	switch (notify_cmd) {
 		case STA_NOTIFY_SLEEP:
@@ -100,7 +100,7 @@ void cw1200_sta_notify(struct ieee80211_hw *dev, struct ieee80211_vif *vif,
 	}
 }
 
-static int cw1200_set_tim_impl(struct cw1200_common *priv, bool multicast)
+static int cw1200_set_tim_impl(struct cw1200_common *priv)
 {
 	struct wsm_template_frame frame = {
 		.frame_type = WSM_FRAME_TYPE_BEACON,
@@ -120,7 +120,7 @@ static int cw1200_set_tim_impl(struct cw1200_common *priv, bool multicast)
 		frame.skb->data[tim_offset + 2] = 0;
 
 		/* Set/reset aid0 bit */
-		if (multicast)
+		if (priv->aid0_bit_set)
 			frame.skb->data[tim_offset + 4] |= 1;
 		else
 			frame.skb->data[tim_offset + 4] &= ~1;
@@ -137,7 +137,7 @@ void cw1200_set_tim_work(struct work_struct *work)
 {
 	struct cw1200_common *priv =
 		container_of(work, struct cw1200_common, set_tim_work);
-	(void)cw1200_set_tim_impl(priv, !priv->suspend_multicast);
+	(void)cw1200_set_tim_impl(priv);
 }
 
 int cw1200_set_tim(struct ieee80211_hw *dev, struct ieee80211_sta *sta,
@@ -429,10 +429,16 @@ void cw1200_multicast_start_work(struct work_struct *work)
 	struct cw1200_common *priv =
 		container_of(work, struct cw1200_common, multicast_start_work);
 
-        (void)cw1200_set_tim_impl(priv, true);
-	priv->suspend_multicast = false;
-	wsm_unlock_tx(priv);
-	cw1200_bh_wakeup(priv);
+	bool store_timestamp = !priv->aid0_bit_set;
+
+	priv->aid0_bit_set = true;
+	cw1200_set_tim_impl(priv);
+	if (store_timestamp) {
+		unsigned long now = jiffies;
+		if (unlikely(!now))
+			++now;
+		priv->aid0_bit_timestamp = now;
+	}
 }
 
 void cw1200_multicast_stop_work(struct work_struct *work)
@@ -440,11 +446,12 @@ void cw1200_multicast_stop_work(struct work_struct *work)
         struct cw1200_common *priv =
                 container_of(work, struct cw1200_common, multicast_stop_work);
 
-	/* Flush to make sure DTIM beacon and frames are sent. */
-	wsm_flush_tx(priv);
-	priv->suspend_multicast = true;
-        (void)cw1200_set_tim_impl(priv, false);
+	/* Flush to make sure frames are sent. */
+	wsm_lock_tx(priv);
 	wsm_unlock_tx(priv);
+
+	priv->aid0_bit_set = false;
+	cw1200_set_tim_impl(priv);
 }
 
 int cw1200_ampdu_action(struct ieee80211_hw *hw,
@@ -465,9 +472,8 @@ int cw1200_ampdu_action(struct ieee80211_hw *hw,
 void cw1200_suspend_resume(struct cw1200_common *priv,
 			  struct wsm_suspend_resume *arg)
 {
-	int queue = 1 << wsm_queue_id_to_linux(arg->queue);
-	u32 unicast = 1 << arg->link_id;
-	u32 after_dtim = 1 << CW1200_LINK_ID_AFTER_DTIM;
+	int queue = BIT(wsm_queue_id_to_linux(arg->queue));
+	u32 unicast = BIT(arg->link_id);
 	u32 wakeup_required = 0;
 	u32 set = 0;
 	u32 clear;
@@ -475,7 +481,7 @@ void cw1200_suspend_resume(struct cw1200_common *priv,
 	int i;
 
 	if (!arg->link_id) /* For all links */
-		unicast = (1 << (CW1200_MAX_STA_IN_AP_MODE + 1)) - 2;
+		unicast = BIT(CW1200_MAX_STA_IN_AP_MODE + 1) - 2;
 
 	ap_printk(KERN_DEBUG "[AP] %s: %s\n",
 		arg->stop ? "stop" : "start",
@@ -483,20 +489,24 @@ void cw1200_suspend_resume(struct cw1200_common *priv,
 
 	if (arg->multicast) {
 		if (arg->stop) {
-			wsm_lock_tx_async(priv);
 			queue_work(priv->workqueue,
 				&priv->multicast_stop_work);
 		} else {
-			/* Handle only if there is data to be sent */
-			for (i = 0; i < 4; ++i) {
-				if (cw1200_queue_get_num_queued(
-						&priv->tx_queue[i],
-						after_dtim)) {
-					wsm_lock_tx_async(priv);
-					queue_work(priv->workqueue,
-						&priv->multicast_start_work);
-					break;
-				}
+			/* Handle only if there is data to be sent
+			* and aid0 is set in the beacon. */
+			unsigned long timestamp =
+					priv->aid0_bit_timestamp;
+			/* 20ms grace interval is used to make sure
+			 * the beacon was actually sent by hardware. */
+			static const unsigned long grace_interval =
+					HZ * 20 / 1000;
+
+			if (timestamp &&
+				jiffies - timestamp > grace_interval) {
+
+				priv->aid0_bit_timestamp = 0;
+				priv->suspend_multicast = false;
+				cw1200_bh_wakeup(priv);
 			}
 		}
 	} else {
@@ -511,7 +521,7 @@ void cw1200_suspend_resume(struct cw1200_common *priv,
 		queue = 0x0F;
 
 		for (i = 0; i < 4; ++i) {
-			if (!(queue & (1 << i)))
+			if (!(queue & BIT(i)))
 				continue;
 
 			tx_suspend_mask = priv->tx_suspend_mask[i];
diff --git a/drivers/staging/cw1200/cw1200.h b/drivers/staging/cw1200/cw1200.h
index 68d82874765..9ed86cd39a1 100644
--- a/drivers/staging/cw1200/cw1200.h
+++ b/drivers/staging/cw1200/cw1200.h
@@ -167,6 +167,10 @@ struct cw1200_common {
 	u32			tx_suspend_mask[4];
 	u32			sta_asleep_mask;
 	bool			suspend_multicast;
+	bool			aid0_bit_set;
+	unsigned long		aid0_bit_timestamp;
+	spinlock_t		buffered_multicasts_lock;
+	bool			buffered_multicasts;
 	struct work_struct	set_tim_work;
 	struct work_struct	multicast_start_work;
 	struct work_struct	multicast_stop_work;
diff --git a/drivers/staging/cw1200/cw1200_sdio.c b/drivers/staging/cw1200/cw1200_sdio.c
index cf5c5c185be..279dff144ae 100644
--- a/drivers/staging/cw1200/cw1200_sdio.c
+++ b/drivers/staging/cw1200/cw1200_sdio.c
@@ -121,10 +121,10 @@ static int cw1200_request_irq(struct sbus_priv *self,
 		goto set_func;
 
 	/* Master interrupt enable ... */
-	cccr |= 1;
+	cccr |= BIT(0);
 
 	/* ... for our function */
-	cccr |= 1 << func_num;
+	cccr |= BIT(func_num);
 
 	sdio_writeb(self->func, cccr, SDIO_CCCR_IENx, &ret);
 	if (WARN_ON(ret))
diff --git a/drivers/staging/cw1200/main.c b/drivers/staging/cw1200/main.c
index 8a621ff2b06..ded4fe9927c 100644
--- a/drivers/staging/cw1200/main.c
+++ b/drivers/staging/cw1200/main.c
@@ -319,6 +319,7 @@ struct ieee80211_hw *cw1200_init_common(size_t priv_data_len)
 	INIT_DELAYED_WORK(&priv->connection_loss_work,
 			  cw1200_connection_loss_work);
 	INIT_WORK(&priv->tx_failure_work, cw1200_tx_failure_work);
+	spin_lock_init(&priv->buffered_multicasts_lock);
 	INIT_WORK(&priv->set_tim_work, cw1200_set_tim_work);
 	INIT_WORK(&priv->multicast_start_work, cw1200_multicast_start_work);
 	INIT_WORK(&priv->multicast_stop_work, cw1200_multicast_stop_work);
diff --git a/drivers/staging/cw1200/sta.c b/drivers/staging/cw1200/sta.c
index fe2cd5cc982..9cb801acefb 100644
--- a/drivers/staging/cw1200/sta.c
+++ b/drivers/staging/cw1200/sta.c
@@ -1076,10 +1076,12 @@ static inline int cw1200_enable_listening(struct cw1200_common *priv)
 
 static inline int cw1200_disable_listening(struct cw1200_common *priv)
 {
+	int ret;
 	struct wsm_reset reset = {
 		.reset_statistics = true,
 	};
-	return wsm_reset(priv, &reset);
+	ret = wsm_reset(priv, &reset);
+	return ret;
 }
 
 void cw1200_update_listening(struct cw1200_common *priv, bool enabled)
diff --git a/drivers/staging/cw1200/txrx.c b/drivers/staging/cw1200/txrx.c
index 7616c237a88..5439947d742 100644
--- a/drivers/staging/cw1200/txrx.c
+++ b/drivers/staging/cw1200/txrx.c
@@ -350,8 +350,8 @@ u32 cw1200_rate_mask_to_wsm(struct cw1200_common *priv, u32 rates)
 	u32 ret = 0;
 	int i;
 	for (i = 0; i < 32; ++i) {
-		if (rates & (1 << i))
-			ret |= 1 << priv->rates[i].hw_value;
+		if (rates & BIT(i))
+			ret |= BIT(priv->rates[i].hw_value);
 	}
 	return ret;
 }
@@ -422,10 +422,12 @@ void cw1200_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 		(struct ieee80211_hdr *)skb->data;
 	struct cw1200_sta_priv *sta_priv =
 		(struct cw1200_sta_priv *)&tx_info->control.sta->drv_priv;
+	bool obtain_lock;
 	int link_id = 0;
 	int ret;
 
-	if (tx_info->flags | IEEE80211_TX_CTL_SEND_AFTER_DTIM)
+	if ((tx_info->flags | IEEE80211_TX_CTL_SEND_AFTER_DTIM) &&
+			(priv->mode == NL80211_IFTYPE_AP))
 		link_id = CW1200_LINK_ID_AFTER_DTIM;
 	else if (tx_info->control.sta)
 		link_id = sta_priv->link_id;
@@ -510,8 +512,22 @@ void cw1200_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 		if (cw1200_handle_action_tx(priv, skb))
 			goto drop;
 
+	obtain_lock = (link_id == CW1200_LINK_ID_AFTER_DTIM);
+
+	if (obtain_lock)
+		spin_lock_bh(&priv->buffered_multicasts_lock);
+
+	if (link_id == CW1200_LINK_ID_AFTER_DTIM &&
+			!priv->buffered_multicasts) {
+		priv->buffered_multicasts = true;
+		queue_work(priv->workqueue,
+				&priv->multicast_start_work);
+	}
 	ret = cw1200_queue_put(&priv->tx_queue[queue], priv, skb,
 			link_id);
+	if (obtain_lock)
+		spin_unlock_bh(&priv->buffered_multicasts_lock);
+
 	if (!WARN_ON(ret))
 		cw1200_bh_wakeup(priv);
 	else
@@ -773,16 +789,16 @@ int cw1200_alloc_key(struct cw1200_common *priv)
 	if (idx < 0 || idx > WSM_KEY_MAX_INDEX)
 		return -1;
 
-	priv->key_map |= 1 << idx;
+	priv->key_map |= BIT(idx);
 	priv->keys[idx].entryIndex = idx;
 	return idx;
 }
 
 void cw1200_free_key(struct cw1200_common *priv, int idx)
 {
-	BUG_ON(!(priv->key_map & (1 << idx)));
+	BUG_ON(!(priv->key_map & BIT(idx)));
 	memset(&priv->keys[idx], 0, sizeof(priv->keys[idx]));
-	priv->key_map &= ~(1 << idx);
+	priv->key_map &= ~BIT(idx);
 }
 
 void cw1200_free_keys(struct cw1200_common *priv)
@@ -795,7 +811,7 @@ int cw1200_upload_keys(struct cw1200_common *priv)
 {
 	int idx, ret = 0;
 	for (idx = 0; idx <= WSM_KEY_MAX_INDEX; ++idx)
-		if (priv->key_map & (1 << idx)) {
+		if (priv->key_map & BIT(idx)) {
 			ret = wsm_add_key(priv, &priv->keys[idx]);
 			if (ret < 0)
 				break;
diff --git a/drivers/staging/cw1200/wsm.c b/drivers/staging/cw1200/wsm.c
index 604fa067ccc..6f40934ed8f 100644
--- a/drivers/staging/cw1200/wsm.c
+++ b/drivers/staging/cw1200/wsm.c
@@ -1423,7 +1423,7 @@ static int wsm_get_tx_queue_and_mask(struct cw1200_common *priv,
 
 	/* Search for a queue with multicast frames buffered */
 	if (priv->sta_asleep_mask && !priv->suspend_multicast) {
-		tx_allowed_mask = 1 << CW1200_LINK_ID_AFTER_DTIM;
+		tx_allowed_mask = BIT(CW1200_LINK_ID_AFTER_DTIM);
 		for (i = 0; i < 4; ++i) {
 			mcasts += cw1200_queue_get_num_queued(
 					&priv->tx_queue[i], tx_allowed_mask);
@@ -1443,7 +1443,7 @@ static int wsm_get_tx_queue_and_mask(struct cw1200_common *priv,
 		if (priv->sta_asleep_mask) {
 			tx_allowed_mask |= ~priv->tx_suspend_mask[i];
 		} else {
-			tx_allowed_mask |= 1 << CW1200_LINK_ID_AFTER_DTIM;
+			tx_allowed_mask |= BIT(CW1200_LINK_ID_AFTER_DTIM);
 		}
 		if (cw1200_queue_get_num_queued(
 				queue, tx_allowed_mask))
@@ -1486,13 +1486,39 @@ int wsm_get_tx(struct cw1200_common *priv, u8 **data,
 		spin_unlock(&priv->wsm_cmd.lock);
 	} else {
 		for (;;) {
+			bool obtain_lock = priv->sta_asleep_mask &&
+					!priv->suspend_multicast;
+			int ret;
+
 			if (atomic_add_return(0, &priv->tx_lock))
 				break;
 
-			if (wsm_get_tx_queue_and_mask(priv, &queue,
-					&tx_allowed_mask, &more))
+			if (obtain_lock)
+				spin_lock_bh(
+					&priv->buffered_multicasts_lock);
+
+			ret = wsm_get_tx_queue_and_mask(priv, &queue,
+					&tx_allowed_mask, &more);
+
+			if (priv->buffered_multicasts &&
+					priv->sta_asleep_mask &&
+					!priv->suspend_multicast &&
+					(ret || tx_allowed_mask !=
+					 BIT(CW1200_LINK_ID_AFTER_DTIM))) {
+				priv->buffered_multicasts = false;
+				priv->suspend_multicast = true;
+				queue_work(priv->workqueue,
+					&priv->multicast_stop_work);
+			}
+
+			if (obtain_lock)
+				spin_unlock_bh(
+					&priv->buffered_multicasts_lock);
+
+			if (ret)
 				break;
 
+
 			if (cw1200_queue_get(queue,
 					tx_allowed_mask,
 					&wsm, &tx_info))
@@ -1513,6 +1539,7 @@ int wsm_get_tx(struct cw1200_common *priv, u8 **data,
 
 			*data = (u8 *)wsm;
 			*tx_len = __le16_to_cpu(wsm->hdr.len);
+
 			if (more) {
 				struct ieee80211_hdr *hdr =
 					(struct ieee80211_hdr *) &wsm[1];
@@ -1521,12 +1548,6 @@ int wsm_get_tx(struct cw1200_common *priv, u8 **data,
 				 *  to inform PS STAs */
 				hdr->frame_control |=
 					cpu_to_le16(IEEE80211_FCTL_MOREDATA);
-			} else if (priv->mode == NL80211_IFTYPE_AP &&
-					!priv->suspend_multicast) {
-				priv->suspend_multicast = true;
-				wsm_lock_tx_async(priv);
-				queue_work(priv->workqueue,
-					&priv->multicast_stop_work);
 			}
 
 			wsm_printk(KERN_DEBUG "[WSM] >>> 0x%.4X (%d) %p %c\n",
diff --git a/drivers/staging/cw1200/wsm.h b/drivers/staging/cw1200/wsm.h
index 97989a01f1c..8a861f918ad 100644
--- a/drivers/staging/cw1200/wsm.h
+++ b/drivers/staging/cw1200/wsm.h
@@ -1292,7 +1292,7 @@ static inline int wsm_set_template_frame(struct cw1200_common *priv,
 	u8 *p = skb_push(arg->skb, 4);
 	p[0] = arg->frame_type;
 	p[1] = arg->rate;
-	((u16 *) p)[1] = __cpu_to_le32(arg->skb->len - 4);
+	((u16 *) p)[1] = __cpu_to_le16(arg->skb->len - 4);
 	ret = wsm_write_mib(priv, WSM_MIB_ID_TEMPLATE_FRAME, p, arg->skb->len);
 	skb_pull(arg->skb, 4);
 	return ret;