cw1200: PSPOLL is supported.

Handling of PSPOLL frames in SoftAP mode was missing until now.
This commit implements PSPOLL support.

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

6 files changed, 61 insertions, 5 deletions

diff --git a/drivers/staging/cw1200/ap.c b/drivers/staging/cw1200/ap.c
index 6fe254802c0..28cb11faa35 100755
--- a/drivers/staging/cw1200/ap.c
+++ b/drivers/staging/cw1200/ap.c
@@ -120,7 +120,8 @@ static int cw1200_set_tim_impl(struct cw1200_common *priv, bool aid0_bit_set)
 	};
 	u16 tim_offset, tim_length;
 
-	ap_printk(KERN_DEBUG "[AP] %s.\n", __func__);
+	ap_printk(KERN_DEBUG "[AP] %s mcast: %s.\n",
+		__func__, aid0_bit_set ? "ena" : "dis");
 
 	frame.skb = ieee80211_beacon_get_tim(priv->hw, priv->vif,
 			&tim_offset, &tim_length);
diff --git a/drivers/staging/cw1200/cw1200.h b/drivers/staging/cw1200/cw1200.h
index 1c5fb561492..9bb68541035 100644
--- a/drivers/staging/cw1200/cw1200.h
+++ b/drivers/staging/cw1200/cw1200.h
@@ -172,6 +172,7 @@ struct cw1200_common {
 	u32			link_id_map;
 	u32			tx_suspend_mask[4];
 	u32			sta_asleep_mask;
+	u32			pspoll_mask;
 	bool			aid0_bit_set;
 	spinlock_t		buffered_multicasts_lock;
 	bool			buffered_multicasts;
diff --git a/drivers/staging/cw1200/debug.c b/drivers/staging/cw1200/debug.c
index 612c74a7174..c918d3f8291 100644
--- a/drivers/staging/cw1200/debug.c
+++ b/drivers/staging/cw1200/debug.c
@@ -217,6 +217,8 @@ static int cw1200_status_show(struct seq_file *seq, void *v)
 		priv->link_id_map);
 	cw1200_debug_print_map(seq, priv, "Asleep map: ",
 		priv->sta_asleep_mask);
+	cw1200_debug_print_map(seq, priv, "PSPOLL map: ",
+		priv->pspoll_mask);
 
 	seq_puts(seq, "\n");
 
diff --git a/drivers/staging/cw1200/sta.c b/drivers/staging/cw1200/sta.c
index c1cedf6db52..df9a8ead315 100644
--- a/drivers/staging/cw1200/sta.c
+++ b/drivers/staging/cw1200/sta.c
@@ -136,6 +136,7 @@ void cw1200_stop(struct ieee80211_hw *dev)
 		priv->tx_multicast = false;
 		priv->aid0_bit_set = false;
 		priv->buffered_multicasts = false;
+		priv->pspoll_mask = 0;
 		wsm_reset(priv, &reset);
 		break;
 	case CW1200_JOIN_STATUS_MONITOR:
diff --git a/drivers/staging/cw1200/txrx.c b/drivers/staging/cw1200/txrx.c
index 6cae1a046d6..5201ffe65ab 100644
--- a/drivers/staging/cw1200/txrx.c
+++ b/drivers/staging/cw1200/txrx.c
@@ -425,12 +425,12 @@ void cw1200_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 	int link_id = 0;
 	int ret;
 
-	if ((tx_info->flags | IEEE80211_TX_CTL_SEND_AFTER_DTIM) &&
+	if (tx_info->control.sta)
+		link_id = sta_priv->link_id;
+	else if ((tx_info->flags | IEEE80211_TX_CTL_SEND_AFTER_DTIM) &&
 			(priv->mode == NL80211_IFTYPE_AP) &&
 			priv->enable_beacon)
 		link_id = CW1200_LINK_ID_AFTER_DTIM;
-	else if (tx_info->control.sta)
-		link_id = sta_priv->link_id;
 
 	txrx_printk(KERN_DEBUG "[TX] TX %d bytes (queue: %d, link_id: %d).\n",
 			skb->len, queue, link_id);
@@ -568,6 +568,50 @@ static int cw1200_handle_action_tx(struct cw1200_common *priv,
 	return 0;
 }
 
+static int cw1200_handle_pspoll(struct cw1200_common *priv,
+				struct sk_buff *skb)
+{
+	struct ieee80211_sta *sta;
+	struct cw1200_sta_priv *sta_priv;
+	struct ieee80211_pspoll *pspoll =
+		(struct ieee80211_pspoll *) skb->data;
+	u32 pspoll_mask;
+	int drop = 1;
+	int i;
+
+	if (priv->join_status != CW1200_JOIN_STATUS_AP)
+		goto done;
+	if (memcmp(priv->vif->addr, pspoll->bssid, ETH_ALEN))
+		goto done;
+
+	rcu_read_lock();
+	sta = ieee80211_find_sta(priv->vif, pspoll->ta);
+	if (!sta) {
+		rcu_read_unlock();
+		goto done;
+	}
+	sta_priv = (struct cw1200_sta_priv *)&sta->drv_priv;
+	pspoll_mask = BIT(sta_priv->link_id);
+	rcu_read_unlock();
+
+	priv->pspoll_mask |= pspoll_mask;
+	drop = 0;
+
+	/* Do not report pspols if data for given link id is
+	 * queued already. */
+	for (i = 0; i < 4; ++i) {
+		if (cw1200_queue_get_num_queued(
+				&priv->tx_queue[i],
+				pspoll_mask)) {
+			cw1200_bh_wakeup(priv);
+			drop = 1;
+			break;
+		}
+	}
+done:
+	return drop;
+}
+
 /* ******************************************************************** */
 
 void cw1200_tx_confirm_cb(struct cw1200_common *priv,
@@ -697,13 +741,18 @@ void cw1200_rx_cb(struct cw1200_common *priv,
 		}
 	}
 
-	if (skb->len < sizeof(struct ieee80211_hdr_3addr)) {
+	if (skb->len < sizeof(struct ieee80211_pspoll)) {
 		wiphy_warn(priv->hw->wiphy, "Mailformed SDU rx'ed. "
 				"Size is lesser than IEEE header.\n");
 		goto drop;
 	}
 
 	frame_control = *(__le16*)skb->data;
+
+	if (unlikely(ieee80211_is_pspoll(frame_control)))
+		if (cw1200_handle_pspoll(priv, skb))
+			goto drop;
+
 	hdr->mactime = 0; /* Not supported by WSM */
 	hdr->band = (arg->channelNumber > 14) ?
 			IEEE80211_BAND_5GHZ : IEEE80211_BAND_2GHZ;
diff --git a/drivers/staging/cw1200/wsm.c b/drivers/staging/cw1200/wsm.c
index 970b3cd9405..57bfa47efcd 100644
--- a/drivers/staging/cw1200/wsm.c
+++ b/drivers/staging/cw1200/wsm.c
@@ -1450,6 +1450,7 @@ static int wsm_get_tx_queue_and_mask(struct cw1200_common *priv,
 		tx_allowed_mask = ~priv->sta_asleep_mask;
 		if (priv->sta_asleep_mask) {
 			tx_allowed_mask |= ~priv->tx_suspend_mask[i];
+			tx_allowed_mask |= priv->pspoll_mask;
 			tx_allowed_mask &= ~BIT(CW1200_LINK_ID_AFTER_DTIM);
 		} else {
 			tx_allowed_mask |= BIT(CW1200_LINK_ID_AFTER_DTIM);
@@ -1537,6 +1538,7 @@ int wsm_get_tx(struct cw1200_common *priv, u8 **data,
 					~WSM_TX_LINK_ID(WSM_TX_LINK_ID_MAX));
 				wsm->hdr.id |= cpu_to_le16(
 					WSM_TX_LINK_ID(sta_priv->link_id));
+				priv->pspoll_mask &= ~BIT(sta_priv->link_id);
 			}
 
 			*data = (u8 *)wsm;