cw1200: UAPSD in AP mode is implemented.

U-APSD in SoftAP is fully offloaded to the mac80211 wireless stack.
Driver provides a transparent virtual link (with link_id = CW1200_LINK_ID_UAPSD)
which bypases powersave buffering. Mac80211 stack takes care to provide UAPSD
data when it is needed.

ST-Ericsson ID: 355584

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

7 files changed, 40 insertions, 28 deletions

diff --git a/drivers/staging/cw1200/cw1200.h b/drivers/staging/cw1200/cw1200.h
index 400fd4a44aa..24bf929e4a3 100644
--- a/drivers/staging/cw1200/cw1200.h
+++ b/drivers/staging/cw1200/cw1200.h
@@ -46,6 +46,8 @@
 
 #define CW1200_MAX_STA_IN_AP_MODE	(5)
 #define CW1200_LINK_ID_AFTER_DTIM	(CW1200_MAX_STA_IN_AP_MODE + 1)
+#define CW1200_LINK_ID_UAPSD		(CW1200_MAX_STA_IN_AP_MODE + 2)
+#define CW1200_LINK_ID_MAX		(CW1200_MAX_STA_IN_AP_MODE + 3)
 #define CW1200_MAX_REQUEUE_ATTEMPTS	(5)
 
 /* Please keep order */
diff --git a/drivers/staging/cw1200/debug.c b/drivers/staging/cw1200/debug.c
index 2d7adfe8b4c..2cfb0ee17cb 100644
--- a/drivers/staging/cw1200/debug.c
+++ b/drivers/staging/cw1200/debug.c
@@ -95,7 +95,7 @@ static void cw1200_debug_print_map(struct seq_file *seq,
 	seq_printf(seq, "%s0-> ", label);
 	for (i = 0; i < priv->tx_queue_stats.map_capacity; ++i)
 		seq_printf(seq, "%s ", (map & BIT(i)) ? "**" : "..");
-	seq_printf(seq, "<-%d\n", priv->tx_queue_stats.map_capacity);
+	seq_printf(seq, "<-%d\n", priv->tx_queue_stats.map_capacity - 1);
 }
 
 static int cw1200_status_show(struct seq_file *seq, void *v)
diff --git a/drivers/staging/cw1200/main.c b/drivers/staging/cw1200/main.c
index 95149da6081..d8e4ecf1af4 100644
--- a/drivers/staging/cw1200/main.c
+++ b/drivers/staging/cw1200/main.c
@@ -277,6 +277,8 @@ struct ieee80211_hw *cw1200_init_common(size_t priv_data_len)
 					  WIPHY_WOWLAN_DISCONNECT;
 	hw->wiphy->wowlan.n_patterns = 0;
 
+	hw->wiphy->flags |= WIPHY_FLAG_SUPPORTS_UAPSD;
+
 	hw->channel_change_time = 1000;	/* TODO: find actual value */
 	/* priv->beacon_req_id = cpu_to_le32(0); */
 	hw->queues = 4;
@@ -341,7 +343,7 @@ struct ieee80211_hw *cw1200_init_common(size_t priv_data_len)
 	}
 
 	if (unlikely(cw1200_queue_stats_init(&priv->tx_queue_stats,
-			CW1200_LINK_ID_AFTER_DTIM + 1))) {
+			CW1200_LINK_ID_MAX))) {
 		ieee80211_free_hw(hw);
 		return NULL;
 	}
diff --git a/drivers/staging/cw1200/queue.c b/drivers/staging/cw1200/queue.c
index e09984583f6..2ba26f32178 100644
--- a/drivers/staging/cw1200/queue.c
+++ b/drivers/staging/cw1200/queue.c
@@ -28,7 +28,8 @@
 	 */
 	u8			generation;
 	u8			link_id;
-	u8			reserved[2];
+	u8			raw_link_id;
+	u8			reserved[1];
 };
 
 static inline void __cw1200_queue_lock(struct cw1200_queue *queue,
@@ -199,7 +200,7 @@ size_t cw1200_queue_get_num_queued(struct cw1200_queue *queue,
 }
 
 int cw1200_queue_put(struct cw1200_queue *queue, struct cw1200_common *priv,
-			struct sk_buff *skb, struct tx_info *txinfo)
+			struct sk_buff *skb, struct tx_info *txinfo, u8 raw_link_id)
 {
 	int ret;
 	struct wsm_tx *wsm;
@@ -228,6 +229,7 @@ int cw1200_queue_put(struct cw1200_queue *queue, struct cw1200_common *priv,
 			item->generation, item - queue->pool);
 		wsm->packetID = __cpu_to_le32(item->packetID);
 		item->link_id = link_id;
+		item->raw_link_id = raw_link_id;
 
 		++queue->num_queued;
 		++queue->link_map_cache[link_id];
@@ -270,7 +272,7 @@ int cw1200_queue_get(struct cw1200_queue *queue,
 	if (!WARN_ON(ret)) {
 		*tx = (struct wsm_tx *)item->skb->data;
 		*tx_info = IEEE80211_SKB_CB(item->skb);
-		*link_id = item->link_id;
+		*link_id = item->raw_link_id;
 		list_move_tail(&item->head, &queue->pending);
 		++queue->num_pending;
 		--queue->link_map_cache[item->link_id];
diff --git a/drivers/staging/cw1200/queue.h b/drivers/staging/cw1200/queue.h
index 4abea64aa66..3ddfdaf910e 100644
--- a/drivers/staging/cw1200/queue.h
+++ b/drivers/staging/cw1200/queue.h
@@ -61,7 +61,7 @@ void cw1200_queue_deinit(struct cw1200_queue *queue);
 size_t cw1200_queue_get_num_queued(struct cw1200_queue *queue,
 				   u32 link_id_map);
 int cw1200_queue_put(struct cw1200_queue *queue, struct cw1200_common *cw1200,
-			struct sk_buff *skb,  struct tx_info *txinfo);
+			struct sk_buff *skb,  struct tx_info *txinfo, u8 raw_link_id);
 int cw1200_queue_get(struct cw1200_queue *queue,
 		     u32 link_id_map,
 		     struct wsm_tx **tx,
diff --git a/drivers/staging/cw1200/txrx.c b/drivers/staging/cw1200/txrx.c
index 3e8b84fa43c..5cd1857dd17 100644
--- a/drivers/staging/cw1200/txrx.c
+++ b/drivers/staging/cw1200/txrx.c
@@ -451,45 +451,52 @@ void cw1200_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 	const u8 *da = ieee80211_get_DA(hdr);
 	struct cw1200_sta_priv *sta_priv =
 		(struct cw1200_sta_priv *)&tx_info->control.sta->drv_priv;
-	int link_id;
+	int link_id, raw_link_id;
 	int ret;
 	int i;
 	struct tx_info txinfo;
 
 	if (likely(tx_info->control.sta && sta_priv->link_id))
-		link_id = sta_priv->link_id;
+		raw_link_id = link_id = sta_priv->link_id;
 	else if (priv->mode != NL80211_IFTYPE_AP)
-		link_id = 0;
+		raw_link_id = link_id = 0;
 	else if (is_multicast_ether_addr(da)) {
-		if (priv->enable_beacon)
+		if (priv->enable_beacon) {
+			raw_link_id = 0;
 			link_id = CW1200_LINK_ID_AFTER_DTIM;
-		else
-			link_id = 0;
+		} else {
+			raw_link_id = link_id = 0;
+		}
 	} else {
-		link_id = cw1200_find_link_id(priv, da);
-		if (!link_id)
-			link_id = cw1200_alloc_link_id(priv, da);
-		if (!link_id) {
+		raw_link_id = cw1200_find_link_id(priv, da);
+		if (!raw_link_id)
+			raw_link_id = cw1200_alloc_link_id(priv, da);
+		if (!raw_link_id) {
 			wiphy_err(priv->hw->wiphy,
 				"%s: No more link IDs available.\n",
 				__func__);
 			goto err;
 		}
+		if (tx_info->control.sta &&
+			(tx_info->control.sta->uapsd_queues & BIT(queue)))
+			link_id = CW1200_LINK_ID_UAPSD;
+		else
+			link_id = raw_link_id;
 	}
-	if (link_id && link_id <= CW1200_MAX_STA_IN_AP_MODE)
-		priv->link_id_db[link_id - 1].timestamp = jiffies;
+	if (raw_link_id)
+		priv->link_id_db[raw_link_id - 1].timestamp = jiffies;
 
-	txrx_printk(KERN_DEBUG "[TX] TX %d bytes (queue: %d, link_id: %d).\n",
-			skb->len, queue, link_id);
+	txrx_printk(KERN_DEBUG "[TX] TX %d bytes (queue: %d, link_id: %d (%d)).\n",
+			skb->len, queue, link_id, raw_link_id);
 
 	if (WARN_ON(queue >= 4))
 		goto err;
 
 	if (unlikely(ieee80211_is_auth(hdr->frame_control))) {
 		spin_lock_bh(&priv->buffered_multicasts_lock);
-		priv->sta_asleep_mask &= ~BIT(link_id);
+		priv->sta_asleep_mask &= ~BIT(raw_link_id);
 		for (i = 0; i < 4; ++i)
-			priv->tx_suspend_mask[i] &= ~BIT(link_id);
+			priv->tx_suspend_mask[i] &= ~BIT(raw_link_id);
 		spin_unlock_bh(&priv->buffered_multicasts_lock);
 	}
 
@@ -607,7 +614,7 @@ void cw1200_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 
 	txinfo.link_id = link_id;
 	ret = cw1200_queue_put(&priv->tx_queue[queue], priv, skb,
-			&txinfo);
+			&txinfo, raw_link_id);
 	spin_unlock_bh(&priv->buffered_multicasts_lock);
 
 	if (!WARN_ON(ret))
diff --git a/drivers/staging/cw1200/wsm.c b/drivers/staging/cw1200/wsm.c
index 6514b9227a7..b05548f34a8 100644
--- a/drivers/staging/cw1200/wsm.c
+++ b/drivers/staging/cw1200/wsm.c
@@ -1313,7 +1313,7 @@ out:
 static bool wsm_handle_tx_data(struct cw1200_common *priv,
 			       const struct wsm_tx *wsm,
 			       const struct ieee80211_tx_info *tx_info,
-			       int *link_id)
+			       int link_id)
 {
 	bool handled = false;
 	const struct ieee80211_hdr *frame =
@@ -1348,9 +1348,7 @@ static bool wsm_handle_tx_data(struct cw1200_common *priv,
 	case NL80211_IFTYPE_AP:
 		if (unlikely(!priv->join_status))
 			action = doDrop;
-		if (*link_id == CW1200_LINK_ID_AFTER_DTIM)
-			*link_id = 0;
-		else if (WARN_ON(!(BIT(*link_id) &
+		else if (WARN_ON(!(BIT(link_id) &
 				(BIT(0) | priv->link_id_map))))
 			action = doDrop;
 		if (cw1200_queue_get_generation(wsm->packetID) >
@@ -1546,6 +1544,7 @@ static int wsm_get_tx_queue_and_mask(struct cw1200_common *priv,
 	for (i = 0; i < 4; ++i) {
 		queue = &priv->tx_queue[i];
 		tx_allowed_mask = ~priv->sta_asleep_mask;
+		tx_allowed_mask |= BIT(CW1200_LINK_ID_UAPSD);
 		if (priv->sta_asleep_mask) {
 			tx_allowed_mask |= ~priv->tx_suspend_mask[i];
 			tx_allowed_mask |= priv->pspoll_mask;
@@ -1625,7 +1624,7 @@ int wsm_get_tx(struct cw1200_common *priv, u8 **data,
 					&wsm, &tx_info, &link_id))
 				continue;
 
-			if (wsm_handle_tx_data(priv, wsm, tx_info, &link_id))
+			if (wsm_handle_tx_data(priv, wsm, tx_info, link_id))
 				continue;  /* Handled by WSM */
 
 			wsm->hdr.id &= __cpu_to_le16(