wlan: cw1200: .flush mac80211 API is implemented.

 - .flush mac80211 API is implemented (expected to be called from
   mac80211 off_channel code).
 - TX queue statistics is implemented.

Signed-off-by: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
Change-Id: If228ceccd856b699236999122c9989d0eca1e98d
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/23604
Reviewed-by: Philippe LANGLAIS <philippe.langlais@stericsson.com>

7 files changed, 221 insertions, 38 deletions

diff --git a/drivers/staging/cw1200/cw1200.h b/drivers/staging/cw1200/cw1200.h
index c4e85893c28..248ca79a404 100644
--- a/drivers/staging/cw1200/cw1200.h
+++ b/drivers/staging/cw1200/cw1200.h
@@ -52,6 +52,7 @@ enum cw1200_join_status {
 
 struct cw1200_common {
 	struct cw1200_queue		tx_queue[4];
+	struct cw1200_queue_stats	tx_queue_stats;
 
 	struct ieee80211_hw		*hw;
 	struct ieee80211_vif		*vif;
diff --git a/drivers/staging/cw1200/main.c b/drivers/staging/cw1200/main.c
index 9cf3a3fba9e..a09832c7343 100644
--- a/drivers/staging/cw1200/main.c
+++ b/drivers/staging/cw1200/main.c
@@ -218,7 +218,11 @@ static const struct ieee80211_ops cw1200_ops = {
 	.conf_tx		= cw1200_conf_tx,
 	.get_stats		= cw1200_get_stats,
 	.ampdu_action		= cw1200_ampdu_action,
-	/* .get_tx_stats		= cw1200_get_tx_stats */
+	.flush			= cw1200_flush,
+	/* Intentionally not offloaded:					*/
+	/*.channel_switch	= cw1200_channel_switch,		*/
+	/*.remain_on_channel	= cw1200_remain_on_channel,		*/
+	/*.cancel_remain_on_channel = cw1200_cancel_remain_on_channel,	*/
 };
 
 struct ieee80211_hw *cw1200_init_common(size_t priv_data_len)
@@ -308,11 +312,18 @@ struct ieee80211_hw *cw1200_init_common(size_t priv_data_len)
 	INIT_WORK(&priv->multicast_start_work, cw1200_multicast_start_work);
 	INIT_WORK(&priv->multicast_stop_work, cw1200_multicast_stop_work);
 
+	if (unlikely(cw1200_queue_stats_init(&priv->tx_queue_stats,
+			CW1200_LINK_ID_AFTER_DTIM + 1))) {
+		ieee80211_free_hw(hw);
+		return NULL;
+	}
+
 	for (i = 0; i < 4; ++i) {
-		if (unlikely(cw1200_queue_init(&priv->tx_queue[i], i,
-				16, CW1200_LINK_ID_AFTER_DTIM + 1))) {
+		if (unlikely(cw1200_queue_init(&priv->tx_queue[i],
+				&priv->tx_queue_stats, i, 16))) {
 			for (; i > 0; i--)
 				cw1200_queue_deinit(&priv->tx_queue[i - 1]);
+			cw1200_queue_stats_deinit(&priv->tx_queue_stats);
 			ieee80211_free_hw(hw);
 			return NULL;
 		}
@@ -364,6 +375,7 @@ EXPORT_SYMBOL_GPL(cw1200_free_common);
 void cw1200_unregister_common(struct ieee80211_hw *dev)
 {
 	struct cw1200_common *priv = dev->priv;
+	int i;
 
 	priv->sbus_ops->irq_unsubscribe(priv->sbus_priv);
 	cw1200_unregister_bh(priv);
@@ -388,6 +400,10 @@ void cw1200_unregister_common(struct ieee80211_hw *dev)
 		dev_kfree_skb(priv->skb_cache);
 		priv->skb_cache = NULL;
 	}
+
+	for (i = 0; i < 4; ++i)
+		cw1200_queue_deinit(&priv->tx_queue[i]);
+	cw1200_queue_stats_deinit(&priv->tx_queue_stats);
 }
 EXPORT_SYMBOL_GPL(cw1200_unregister_common);
 
@@ -421,7 +437,7 @@ int cw1200_probe(const struct sbus_ops *sbus_ops,
 	priv->wsm_cbc.rx = cw1200_rx_cb;
 	priv->wsm_cbc.suspend_resume = cw1200_suspend_resume;
 	/* priv->wsm_cbc.set_pm_complete = cw1200_set_pm_complete_cb; */
-	/* priv->wsm_cbc.channel_switch = cw1200_channel_switch_cb; */
+	priv->wsm_cbc.channel_switch = cw1200_channel_switch_cb;
 
 	err = cw1200_register_bh(priv);
 	if (err)
diff --git a/drivers/staging/cw1200/queue.c b/drivers/staging/cw1200/queue.c
index 3adf4fc3072..f81e46cd4f4 100644
--- a/drivers/staging/cw1200/queue.c
+++ b/drivers/staging/cw1200/queue.c
@@ -21,8 +21,9 @@
 	struct sk_buff		*skb;
 	u32			packetID;
 	/* For safety purposes only. I do not trust device too much.
-	 * It was observed that it indicates TX for a packet several times,
-	 * so it's not enough to use offset or address as an uniquie ID in a
+	 * It was observed (last time quite long time ago) that it
+	 * indicates TX for a packet several times, so it was not enough
+	 * to use offset or address as an uniquie ID in a
 	 * queue.
 	 */
 	u8			generation;
@@ -71,14 +72,32 @@ static inline u32 cw1200_queue_make_packet_id(u8 queue_generation, u8 queue_id,
 		((u32)queue_generation << 24);
 }
 
-int cw1200_queue_init(struct cw1200_queue *queue, u8 queue_id,
-		      size_t capacity, size_t map_capacity)
+int cw1200_queue_stats_init(struct cw1200_queue_stats *stats,
+			    size_t map_capacity)
+{
+	memset(stats, 0, sizeof(*stats));
+	stats->map_capacity = map_capacity;
+	spin_lock_init(&stats->lock);
+	init_waitqueue_head(&stats->wait_link_id_empty);
+
+	stats->link_map_cache = kzalloc(sizeof(int[map_capacity]),
+			GFP_KERNEL);
+	if (!stats->link_map_cache)
+		return -ENOMEM;
+
+	return 0;
+}
+
+int cw1200_queue_init(struct cw1200_queue *queue,
+		      struct cw1200_queue_stats *stats,
+		      u8 queue_id,
+		      size_t capacity)
 {
 	size_t i;
 
 	memset(queue, 0, sizeof(*queue));
+	queue->stats = stats;
 	queue->capacity = capacity;
-	queue->map_capacity = map_capacity;
 	queue->queue_id = queue_id;
 	INIT_LIST_HEAD(&queue->queue);
 	INIT_LIST_HEAD(&queue->pending);
@@ -86,12 +105,12 @@ int cw1200_queue_init(struct cw1200_queue *queue, u8 queue_id,
 	spin_lock_init(&queue->lock);
 
 	queue->pool = kzalloc(sizeof(struct cw1200_queue_item) * capacity,
-				GFP_KERNEL);
+			GFP_KERNEL);
 	if (!queue->pool)
 		return -ENOMEM;
 
-	queue->link_map_cache = kzalloc(sizeof(int[map_capacity]),
-                                GFP_KERNEL);
+	queue->link_map_cache = kzalloc(sizeof(int[stats->map_capacity]),
+			GFP_KERNEL);
 	if (!queue->link_map_cache) {
 		kfree(queue->pool);
 		queue->pool = NULL;
@@ -106,6 +125,9 @@ int cw1200_queue_init(struct cw1200_queue *queue, u8 queue_id,
 
 int cw1200_queue_clear(struct cw1200_queue *queue)
 {
+	int i;
+	struct cw1200_queue_stats *stats = queue->stats;
+
 	spin_lock_bh(&queue->lock);
 	queue->generation++;
 	list_splice_tail_init(&queue->queue, &queue->pending);
@@ -122,39 +144,53 @@ int cw1200_queue_clear(struct cw1200_queue *queue)
 	queue->num_queued = 0;
 	queue->num_pending = 0;
 	queue->num_sent = 0;
-	memset(queue->link_map_cache, 0, sizeof(int[queue->map_capacity]));
+
+	spin_lock_bh(&stats->lock);
+	for (i = 0; i < stats->map_capacity; ++i) {
+		stats->num_queued -= queue->link_map_cache[i];
+		stats->link_map_cache[i] -= queue->link_map_cache[i];
+		queue->link_map_cache[i] = 0;
+	}
+	spin_unlock_bh(&stats->lock);
 	spin_unlock_bh(&queue->lock);
+	wake_up_interruptible(&stats->wait_link_id_empty);
 	return 0;
 }
 
-int cw1200_queue_deinit(struct cw1200_queue *queue)
+void cw1200_queue_stats_deinit(struct cw1200_queue_stats *stats)
+{
+	kfree(stats->link_map_cache);
+	stats->link_map_cache = NULL;
+}
+
+void cw1200_queue_deinit(struct cw1200_queue *queue)
 {
 	cw1200_queue_clear(queue);
 	INIT_LIST_HEAD(&queue->free_pool);
 	kfree(queue->pool);
 	kfree(queue->link_map_cache);
 	queue->pool = NULL;
+	queue->link_map_cache = NULL;
 	queue->capacity = 0;
-
-	return 0;
 }
 
 size_t cw1200_queue_get_num_queued(struct cw1200_queue *queue,
-				   u32 allowed_mask)
+				   u32 link_id_map)
 {
 	size_t ret;
 	int i, bit;
+	size_t map_capacity = queue->stats->map_capacity;
 
-	if (!allowed_mask)
+	if (!link_id_map)
 		return 0;
 
 	spin_lock_bh(&queue->lock);
-	if (likely(allowed_mask == (u32) -1))
+	if (likely(link_id_map == (u32) -1))
 		ret = queue->num_queued - queue->num_pending;
 	else {
 		ret = 0;
-		for (i = 0, bit = 1; i < queue->map_capacity; ++i, bit <<= 1) {
-			if (allowed_mask & bit)
+		for (i = 0, bit = 1; i < map_capacity; ++i, bit <<= 1) {
+			if (link_id_map & bit)
 				ret += queue->link_map_cache[i];
 		}
 	}
@@ -167,13 +203,14 @@ int cw1200_queue_put(struct cw1200_queue *queue, struct cw1200_common *priv,
 {
 	int ret;
 	struct wsm_tx *wsm;
+	struct cw1200_queue_stats *stats = queue->stats;
 
 	wsm = (struct wsm_tx *)skb_push(skb, sizeof(struct wsm_tx));
 	ret = cw1200_skb_to_wsm(priv, skb, wsm);
 	if (ret)
 		return ret;
 
-	if (link_id >= queue->map_capacity)
+	if (link_id >= queue->stats->map_capacity)
 		return -EINVAL;
 
 	spin_lock_bh(&queue->lock);
@@ -193,6 +230,11 @@ int cw1200_queue_put(struct cw1200_queue *queue, struct cw1200_common *priv,
 		++queue->num_queued;
 		++queue->link_map_cache[link_id];
 
+		spin_lock_bh(&stats->lock);
+		++stats->num_queued;
+		++stats->link_map_cache[link_id];
+		spin_unlock_bh(&stats->lock);
+
 		if (queue->num_queued >= queue->capacity) {
 			queue->overfull = true;
 			__cw1200_queue_lock(queue, priv);
@@ -205,16 +247,18 @@ int cw1200_queue_put(struct cw1200_queue *queue, struct cw1200_common *priv,
 }
 
 int cw1200_queue_get(struct cw1200_queue *queue,
-		     u32 allowed_mask,
+		     u32 link_id_map,
 		     struct wsm_tx **tx,
 		     struct ieee80211_tx_info **tx_info)
 {
 	int ret = -ENOENT;
 	struct cw1200_queue_item *item;
+	struct cw1200_queue_stats *stats = queue->stats;
+	bool wakeup_stats = false;
 
 	spin_lock_bh(&queue->lock);
 	list_for_each_entry(item, &queue->queue, head) {
-		if (allowed_mask & (1 << item->link_id)) {
+		if (link_id_map & BIT(item->link_id)) {
 			ret = 0;
 			break;
 		}
@@ -226,8 +270,16 @@ int cw1200_queue_get(struct cw1200_queue *queue,
 		list_move_tail(&item->head, &queue->pending);
 		++queue->num_pending;
 		--queue->link_map_cache[item->link_id];
+
+		spin_lock_bh(&stats->lock);
+		--stats->num_queued;
+		if (!--stats->link_map_cache[item->link_id])
+			wakeup_stats = true;
+		spin_unlock_bh(&stats->lock);
 	}
 	spin_unlock_bh(&queue->lock);
+	if (wakeup_stats)
+		wake_up_interruptible(&stats->wait_link_id_empty);
 	return ret;
 }
 
@@ -236,6 +288,8 @@ int cw1200_queue_requeue(struct cw1200_queue *queue, u32 packetID)
 	int ret = 0;
 	u8 queue_generation, queue_id, item_generation, item_id;
 	struct cw1200_queue_item *item;
+	struct cw1200_queue_stats *stats = queue->stats;
+
 	cw1200_queue_parse_id(packetID, &queue_generation, &queue_id,
 				&item_generation, &item_id);
 
@@ -255,6 +309,12 @@ int cw1200_queue_requeue(struct cw1200_queue *queue, u32 packetID)
 		struct wsm_tx *wsm = (struct wsm_tx *)item->skb->data;
 		--queue->num_pending;
 		++queue->link_map_cache[item->link_id];
+
+		spin_lock_bh(&stats->lock);
+		++stats->num_queued;
+		++stats->link_map_cache[item->link_id];
+		spin_unlock_bh(&stats->lock);
+
 		item->generation = ++item_generation;
 		item->packetID = cw1200_queue_make_packet_id(
 			queue_generation, queue_id, item_generation, item_id);
@@ -267,6 +327,7 @@ int cw1200_queue_requeue(struct cw1200_queue *queue, u32 packetID)
 
 int cw1200_queue_requeue_all(struct cw1200_queue *queue)
 {
+	struct cw1200_queue_stats *stats = queue->stats;
 	spin_lock_bh(&queue->lock);
 	while (!list_empty(&queue->pending)) {
 		struct cw1200_queue_item *item = list_first_entry(
@@ -275,6 +336,12 @@ int cw1200_queue_requeue_all(struct cw1200_queue *queue)
 
 		--queue->num_pending;
 		++queue->link_map_cache[item->link_id];
+
+		spin_lock_bh(&stats->lock);
+		++stats->num_queued;
+		++stats->link_map_cache[item->link_id];
+		spin_unlock_bh(&stats->lock);
+
 		++item->generation;
 		item->packetID = cw1200_queue_make_packet_id(
 			queue->generation, queue->queue_id,
@@ -393,3 +460,26 @@ int cw1200_queue_get_stats(struct cw1200_queue *queue,
 }
 */
 
+bool cw1200_queue_stats_is_empty(struct cw1200_queue_stats *stats,
+				 u32 link_id_map)
+{
+	bool empty = true;
+
+	spin_lock_bh(&stats->lock);
+	if (link_id_map == (u32)-1)
+		empty = stats->num_queued == 0;
+	else {
+		int i;
+		for (i = 0; i < stats->map_capacity; ++i) {
+			if (link_id_map & BIT(i)) {
+				if (stats->link_map_cache[i]) {
+					empty = false;
+					break;
+				}
+			}
+		}
+	}
+	spin_unlock_bh(&stats->lock);
+
+	return empty;
+}
diff --git a/drivers/staging/cw1200/queue.h b/drivers/staging/cw1200/queue.h
index a87505defba..fc5d4613efb 100644
--- a/drivers/staging/cw1200/queue.h
+++ b/drivers/staging/cw1200/queue.h
@@ -19,7 +19,10 @@
 /* extern */ struct cw1200_common;
 /* extern */ struct ieee80211_tx_queue_stats;
 
+/* forward */ struct cw1200_queue_stats;
+
 struct cw1200_queue {
+	struct cw1200_queue_stats *stats;
 	size_t			capacity;
 	size_t			num_queued;
 	size_t			num_pending;
@@ -30,24 +33,36 @@ struct cw1200_queue {
 	struct list_head	pending;
 	int			tx_locked_cnt;
 	int			*link_map_cache;
-	size_t			map_capacity;
 	bool			overfull;
 	spinlock_t		lock;
 	u8			queue_id;
 	u8			generation;
 };
 
-int cw1200_queue_init(struct cw1200_queue *queue, u8 queue_id,
-		      size_t capacity, size_t map_capacity);
+struct cw1200_queue_stats {
+	spinlock_t		lock;
+	int			*link_map_cache;
+	int			num_queued;
+	size_t			map_capacity;
+	wait_queue_head_t	wait_link_id_empty;
+};
+
+int cw1200_queue_stats_init(struct cw1200_queue_stats *stats,
+			    size_t map_capacity);
+int cw1200_queue_init(struct cw1200_queue *queue,
+		      struct cw1200_queue_stats *stats,
+		      u8 queue_id,
+		      size_t capacity);
 int cw1200_queue_clear(struct cw1200_queue *queue);
-int cw1200_queue_deinit(struct cw1200_queue *queue);
+void cw1200_queue_stats_deinit(struct cw1200_queue_stats *stats);
+void cw1200_queue_deinit(struct cw1200_queue *queue);
 
 size_t cw1200_queue_get_num_queued(struct cw1200_queue *queue,
-				   u32 allowed_mask);
+				   u32 link_id_map);
 int cw1200_queue_put(struct cw1200_queue *queue, struct cw1200_common *cw1200,
 			struct sk_buff *skb, u8 link_id);
 int cw1200_queue_get(struct cw1200_queue *queue,
-		     u32 allowed_mask,
+		     u32 link_id_map,
 		     struct wsm_tx **tx,
 		     struct ieee80211_tx_info **tx_info);
 int cw1200_queue_requeue(struct cw1200_queue *queue, u32 packetID);
@@ -61,6 +76,9 @@ void cw1200_queue_lock(struct cw1200_queue *queue,
 void cw1200_queue_unlock(struct cw1200_queue *queue,
 				struct cw1200_common *cw1200);
 
+bool cw1200_queue_stats_is_empty(struct cw1200_queue_stats *stats,
+				 u32 link_id_map);
+
 /* int cw1200_queue_get_stats(struct cw1200_queue *queue,
 struct ieee80211_tx_queue_stats *stats); */
 
diff --git a/drivers/staging/cw1200/sta.c b/drivers/staging/cw1200/sta.c
index 6cebf530f40..4cdcd4a43a6 100644
--- a/drivers/staging/cw1200/sta.c
+++ b/drivers/staging/cw1200/sta.c
@@ -26,6 +26,7 @@
 
 
 static int cw1200_cancel_scan(struct cw1200_common *priv);
+static int __cw1200_flush(struct cw1200_common *priv, bool drop);
 
 static inline void __cw1200_free_event_queue(struct list_head *list)
 {
@@ -251,18 +252,26 @@ int cw1200_config(struct ieee80211_hw *dev, u32 changed)
 
 
 	if ((changed & IEEE80211_CONF_CHANGE_CHANNEL) &&
-		(priv->channel != conf->channel)) {
+			(priv->channel != conf->channel)) {
+		struct ieee80211_channel *ch = conf->channel;
 		struct wsm_switch_channel channel = {
-			.newChannelNumber = conf->channel->hw_value
+			.newChannelNumber = ch->hw_value,
 		};
 		cw1200_cancel_scan(priv);
 		sta_printk(KERN_DEBUG "[STA] Freq %d (wsm ch: %d).\n",
-			conf->channel->center_freq, conf->channel->hw_value);
+			ch->center_freq, ch->hw_value);
 		WARN_ON(wait_event_interruptible_timeout(
 			priv->channel_switch_done,
 			!priv->channel_switch_in_progress, 3 * HZ) <= 0);
-		WARN_ON(wsm_switch_channel(priv, &channel));
-		priv->channel = conf->channel;
+
+		ret = WARN_ON(__cw1200_flush(priv, false));
+		if (!ret) {
+			ret = WARN_ON(wsm_switch_channel(priv, &channel));
+			if (!ret)
+				priv->channel = ch;
+			else
+				wsm_unlock_tx(priv);
+		}
 	}
 
 	if (changed & IEEE80211_CONF_CHANGE_PS) {
@@ -567,6 +576,50 @@ int cw1200_set_rts_threshold(struct ieee80211_hw *hw, u32 value)
 	return ret;
 }
 
+static int __cw1200_flush(struct cw1200_common *priv, bool drop)
+{
+	int i, ret;
+
+	if (drop) {
+		for (i = 0; i < 4; ++i)
+			cw1200_queue_clear(&priv->tx_queue[i]);
+	}
+
+	for (;;) {
+		ret = wait_event_interruptible_timeout(
+				priv->tx_queue_stats.wait_link_id_empty,
+				cw1200_queue_stats_is_empty(
+					&priv->tx_queue_stats, -1),
+				10 * HZ);
+
+		if (unlikely(ret <= 0)) {
+			if (!ret)
+				ret = -ETIMEDOUT;
+			break;
+		}
+
+		wsm_lock_tx(priv);
+		if (unlikely(!cw1200_queue_stats_is_empty(
+				&priv->tx_queue_stats, -1))) {
+			/* Highly unlekely: WSM requeued frames. */
+			wsm_unlock_tx(priv);
+			continue;
+		}
+		break;
+	}
+	return ret;
+}
+
+void cw1200_flush(struct ieee80211_hw *hw, bool drop)
+{
+	struct cw1200_common *priv = hw->priv;
+
+	if (!WARN_ON(__cw1200_flush(priv, drop)))
+		wsm_unlock_tx(priv);
+
+	return;
+}
+
 /* ******************************************************************** */
 /* WSM callbacks							*/
 
@@ -630,6 +683,11 @@ void cw1200_rx_cb(struct cw1200_common *priv,
 	*skb_p = NULL;
 }
 
+void cw1200_channel_switch_cb(struct cw1200_common *priv)
+{
+	wsm_unlock_tx(priv);
+}
+
 void cw1200_free_event_queue(struct cw1200_common *priv)
 {
 	LIST_HEAD(list);
diff --git a/drivers/staging/cw1200/sta.h b/drivers/staging/cw1200/sta.h
index a36127b3843..63cc1793151 100644
--- a/drivers/staging/cw1200/sta.h
+++ b/drivers/staging/cw1200/sta.h
@@ -40,6 +40,8 @@ int cw1200_set_key(struct ieee80211_hw *dev, enum set_key_cmd cmd,
 
 int cw1200_set_rts_threshold(struct ieee80211_hw *hw, u32 value);
 
+void cw1200_flush(struct ieee80211_hw *hw, bool drop);
+
 /* ******************************************************************** */
 /* WSM callbacks							*/
 
@@ -48,7 +50,7 @@ void cw1200_rx_cb(struct cw1200_common *priv,
 		  struct sk_buff **skb_p);
 /* void cw1200_set_pm_complete_cb(struct cw1200_common *priv,
 	struct wsm_set_pm_complete *arg); */
-/* void cw1200_channel_switch_cb(struct cw1200_common *priv); */
+void cw1200_channel_switch_cb(struct cw1200_common *priv);
 
 /* ******************************************************************** */
 /* WSM events								*/
diff --git a/drivers/staging/cw1200/wsm.c b/drivers/staging/cw1200/wsm.c
index 2a07e7e93f8..5c3bd5c12f3 100644
--- a/drivers/staging/cw1200/wsm.c
+++ b/drivers/staging/cw1200/wsm.c
@@ -1499,8 +1499,6 @@ void wsm_txed(struct cw1200_common *priv, u8 *data)
 		priv->wsm_cmd.ptr = NULL;
 		spin_unlock(&priv->wsm_cmd.lock);
 	}
-
-	/* TODO: data queues */
 }
 
 /* ******************************************************************** */