3 files changed, 107 insertions, 16 deletions

diff --git a/sys/compat/linuxkpi/common/include/net/mac80211.h b/sys/compat/linuxkpi/common/include/net/mac80211.h
index fa36bd84ac6e..c4d001b3a7e8 100644
--- a/sys/compat/linuxkpi/common/include/net/mac80211.h
+++ b/sys/compat/linuxkpi/common/include/net/mac80211.h
@@ -1117,6 +1117,8 @@ void linuxkpi_ieee80211_txq_schedule_start(struct ieee80211_hw *, uint8_t);
 struct ieee80211_txq *linuxkpi_ieee80211_next_txq(struct ieee80211_hw *, uint8_t);
 void linuxkpi_ieee80211_schedule_txq(struct ieee80211_hw *,
     struct ieee80211_txq *, bool);
+void linuxkpi_ieee80211_handle_wake_tx_queue(struct ieee80211_hw *,
+	struct ieee80211_txq *);
 
 /* -------------------------------------------------------------------------- */
 
@@ -1681,7 +1683,7 @@ static inline void
 ieee80211_return_txq(struct ieee80211_hw *hw, struct ieee80211_txq *txq,
     bool withoutpkts)
 {
-	linuxkpi_ieee80211_schedule_txq(hw, txq, true);
+	linuxkpi_ieee80211_schedule_txq(hw, txq, withoutpkts);
 }
 
 static inline void
@@ -1706,7 +1708,7 @@ static inline void
 ieee80211_handle_wake_tx_queue(struct ieee80211_hw *hw,
     struct ieee80211_txq *txq)
 {
-	TODO();
+	linuxkpi_ieee80211_handle_wake_tx_queue(hw, txq);
 }
 
 /* -------------------------------------------------------------------------- */
@@ -2197,13 +2199,25 @@ ieee80211_tkip_add_iv(u8 *crypto_hdr, struct ieee80211_key_conf *keyconf,
 	TODO();
 }
 
-static __inline struct sk_buff *
+static inline struct sk_buff *
 ieee80211_tx_dequeue(struct ieee80211_hw *hw, struct ieee80211_txq *txq)
 {
 
 	return (linuxkpi_ieee80211_tx_dequeue(hw, txq));
 }
 
+static inline struct sk_buff *
+ieee80211_tx_dequeue_ni(struct ieee80211_hw *hw, struct ieee80211_txq *txq)
+{
+	struct sk_buff *skb;
+
+	local_bh_disable();
+	skb = linuxkpi_ieee80211_tx_dequeue(hw, txq);
+	local_bh_enable();
+
+	return (skb);
+}
+
 static __inline void
 ieee80211_update_mu_groups(struct ieee80211_vif *vif,
     u_int _i, uint8_t *ms, uint8_t *up)
@@ -2456,13 +2470,6 @@ ieee80211_stop_rx_ba_session_offl(struct ieee80211_vif *vif, uint8_t *addr,
 	TODO();
 }
 
-static __inline struct sk_buff *
-ieee80211_tx_dequeue_ni(struct ieee80211_hw *hw, struct ieee80211_txq *txq)
-{
-	TODO();
-	return (NULL);
-}
-
 static __inline void
 ieee80211_tx_rate_update(struct ieee80211_hw *hw, struct ieee80211_sta *sta,
     struct ieee80211_tx_info *info)
diff --git a/sys/compat/linuxkpi/common/src/linux_80211.c b/sys/compat/linuxkpi/common/src/linux_80211.c
index 5a60380d15aa..b71deedb12b8 100644
--- a/sys/compat/linuxkpi/common/src/linux_80211.c
+++ b/sys/compat/linuxkpi/common/src/linux_80211.c
@@ -330,6 +330,7 @@ lkpi_lsta_alloc(struct ieee80211vap *vap, const uint8_t mac[IEEE80211_ADDR_LEN],
 		ltxq->txq.sta = sta;
 		TAILQ_ELEM_INIT(ltxq, txq_entry);
 		skb_queue_head_init(&ltxq->skbq);
+		LKPI_80211_LTXQ_LOCK_INIT(ltxq);
 		sta->txq[tid] = &ltxq->txq;
 	}
 
@@ -381,8 +382,13 @@ lkpi_lsta_alloc(struct ieee80211vap *vap, const uint8_t mac[IEEE80211_ADDR_LEN],
 	return (lsta);
 
 cleanup:
-	for (; tid >= 0; tid--)
+	for (; tid >= 0; tid--) {
+		struct lkpi_txq *ltxq;
+
+		ltxq = TXQ_TO_LTXQ(sta->txq[tid]);
+		LKPI_80211_LTXQ_LOCK_DESTROY(ltxq);
 		free(sta->txq[tid], M_LKPI80211);
+	}
 	free(lsta, M_LKPI80211);
 	return (NULL);
 }
@@ -976,6 +982,7 @@ lkpi_wake_tx_queues(struct ieee80211_hw *hw, struct ieee80211_sta *sta,
 {
 	struct lkpi_txq *ltxq;
 	int tid;
+	bool ltxq_empty;
 
 	/* Wake up all queues to know they are allocated in the driver. */
 	for (tid = 0; tid < nitems(sta->txq); tid++) {
@@ -994,7 +1001,10 @@ lkpi_wake_tx_queues(struct ieee80211_hw *hw, struct ieee80211_sta *sta,
 		if (dequeue_seen && !ltxq->seen_dequeue)
 			continue;
 
-		if (no_emptyq && skb_queue_empty(&ltxq->skbq))
+		LKPI_80211_LTXQ_LOCK(ltxq);
+		ltxq_empty = skb_queue_empty(&ltxq->skbq);
+		LKPI_80211_LTXQ_UNLOCK(ltxq);
+		if (no_emptyq && ltxq_empty)
 			continue;
 
 		lkpi_80211_mo_wake_tx_queue(hw, sta->txq[tid]);
@@ -3541,6 +3551,7 @@ lkpi_80211_txq_tx_one(struct lkpi_sta *lsta, struct mbuf *m)
 		KASSERT(ltxq != NULL, ("%s: lsta %p sta %p m %p skb %p "
 		    "ltxq %p != NULL\n", __func__, lsta, sta, m, skb, ltxq));
 
+		LKPI_80211_LTXQ_LOCK(ltxq);
 		skb_queue_tail(&ltxq->skbq, skb);
 #ifdef LINUXKPI_DEBUG_80211
 		if (linuxkpi_debug_80211 & D80211_TRACE_TX)
@@ -3553,6 +3564,7 @@ lkpi_80211_txq_tx_one(struct lkpi_sta *lsta, struct mbuf *m)
 			    skb_queue_len(&ltxq->skbq), ltxq->txq.ac,
 			    ltxq->txq.tid, ac, skb->priority, skb->qmap);
 #endif
+		LKPI_80211_LTXQ_UNLOCK(ltxq);
 		lkpi_80211_mo_wake_tx_queue(hw, &ltxq->txq);
 		return;
 	}
@@ -4075,6 +4087,7 @@ linuxkpi_ieee80211_alloc_hw(size_t priv_len, const struct ieee80211_ops *ops)
 
 	LKPI_80211_LHW_LOCK_INIT(lhw);
 	LKPI_80211_LHW_SCAN_LOCK_INIT(lhw);
+	LKPI_80211_LHW_TXQ_LOCK_INIT(lhw);
 	sx_init_flags(&lhw->lvif_sx, "lhw-lvif", SX_RECURSE | SX_DUPOK);
 	TAILQ_INIT(&lhw->lvif_head);
 	for (ac = 0; ac < IEEE80211_NUM_ACS; ac++) {
@@ -4113,9 +4126,10 @@ linuxkpi_ieee80211_iffree(struct ieee80211_hw *hw)
 	lhw->ic = NULL;
 
 	/* Cleanup more of lhw here or in wiphy_free()? */
-	sx_destroy(&lhw->lvif_sx);
-	LKPI_80211_LHW_LOCK_DESTROY(lhw);
+	LKPI_80211_LHW_TXQ_LOCK_DESTROY(lhw);
 	LKPI_80211_LHW_SCAN_LOCK_DESTROY(lhw);
+	LKPI_80211_LHW_LOCK_DESTROY(lhw);
+	sx_destroy(&lhw->lvif_sx);
 	IMPROVE();
 }
 
@@ -5021,7 +5035,11 @@ linuxkpi_ieee80211_tx_dequeue(struct ieee80211_hw *hw,
 		goto stopped;
 	}
 
+	IMPROVE("hw(TX_FRAG_LIST)");
+
+	LKPI_80211_LTXQ_LOCK(ltxq);
 	skb = skb_dequeue(&ltxq->skbq);
+	LKPI_80211_LTXQ_UNLOCK(ltxq);
 
 stopped:
 	return (skb);
@@ -5038,10 +5056,12 @@ linuxkpi_ieee80211_txq_get_depth(struct ieee80211_txq *txq,
 	ltxq = TXQ_TO_LTXQ(txq);
 
 	fc = bc = 0;
+	LKPI_80211_LTXQ_LOCK(ltxq);
 	skb_queue_walk(&ltxq->skbq, skb) {
 		fc++;
 		bc += skb->len;
 	}
+	LKPI_80211_LTXQ_UNLOCK(ltxq);
 	if (frame_cnt)
 		*frame_cnt = fc;
 	if (byte_cnt)
@@ -5594,13 +5614,17 @@ void linuxkpi_ieee80211_schedule_txq(struct ieee80211_hw *hw,
 {
 	struct lkpi_hw *lhw;
 	struct lkpi_txq *ltxq;
+	bool ltxq_empty;
 
 	ltxq = TXQ_TO_LTXQ(txq);
 
 	IMPROVE_TXQ("LOCKING");
 
 	/* Only schedule if work to do or asked to anyway. */
-	if (!withoutpkts && skb_queue_empty(&ltxq->skbq))
+	LKPI_80211_LTXQ_LOCK(ltxq);
+	ltxq_empty = skb_queue_empty(&ltxq->skbq);
+	LKPI_80211_LTXQ_UNLOCK(ltxq);
+	if (!withoutpkts && ltxq_empty)
 		goto out;
 
 	/* Make sure we do not double-schedule. */
@@ -5613,6 +5637,39 @@ out:
 	return;
 }
 
+void
+linuxkpi_ieee80211_handle_wake_tx_queue(struct ieee80211_hw *hw,
+    struct ieee80211_txq *txq)
+{
+	struct lkpi_hw *lhw;
+	struct ieee80211_txq *ntxq;
+	struct ieee80211_tx_control control;
+        struct sk_buff *skb;
+
+	lhw = HW_TO_LHW(hw);
+
+	LKPI_80211_LHW_TXQ_LOCK(lhw);
+	ieee80211_txq_schedule_start(hw, txq->ac);
+	do {
+		ntxq = ieee80211_next_txq(hw, txq->ac);
+		if (ntxq == NULL)
+			break;
+
+		memset(&control, 0, sizeof(control));
+		control.sta = ntxq->sta;
+		do {
+			skb = linuxkpi_ieee80211_tx_dequeue(hw, ntxq);
+			if (skb == NULL)
+				break;
+			lkpi_80211_mo_tx(hw, &control, skb);
+		} while(1);
+
+		ieee80211_return_txq(hw, ntxq, false);
+	} while (1);
+	ieee80211_txq_schedule_end(hw, txq->ac);
+	LKPI_80211_LHW_TXQ_UNLOCK(lhw);
+}
+
 /* -------------------------------------------------------------------------- */
 
 struct lkpi_cfg80211_bss {
diff --git a/sys/compat/linuxkpi/common/src/linux_80211.h b/sys/compat/linuxkpi/common/src/linux_80211.h
index ef1f841e4f22..c9ac19321ab3 100644
--- a/sys/compat/linuxkpi/common/src/linux_80211.h
+++ b/sys/compat/linuxkpi/common/src/linux_80211.h
@@ -109,6 +109,7 @@ struct lkpi_radiotap_rx_hdr {
 struct lkpi_txq {
 	TAILQ_ENTRY(lkpi_txq)	txq_entry;
 
+	struct mtx		ltxq_mtx;
 	bool			seen_dequeue;
 	bool			stopped;
 	uint32_t		txq_generation;
@@ -184,6 +185,7 @@ struct lkpi_hw {	/* name it mac80211_sc? */
 
 	struct sx			sx;
 
+	struct mtx			txq_mtx;
 	uint32_t			txq_generation[IEEE80211_NUM_ACS];
 	TAILQ_HEAD(, lkpi_txq)		scheduled_txqs[IEEE80211_NUM_ACS];
 
@@ -279,13 +281,26 @@ struct lkpi_wiphy {
     mtx_destroy(&(_lhw)->scan_mtx);
 #define	LKPI_80211_LHW_SCAN_LOCK(_lhw)			\
     mtx_lock(&(_lhw)->scan_mtx)
-#define	LKPI_80211_LHW_SCAN_UNLOCK(_lhw)        	\
+#define	LKPI_80211_LHW_SCAN_UNLOCK(_lhw)		\
     mtx_unlock(&(_lhw)->scan_mtx)
 #define	LKPI_80211_LHW_SCAN_LOCK_ASSERT(_lhw)		\
     mtx_assert(&(_lhw)->scan_mtx, MA_OWNED)
 #define	LKPI_80211_LHW_SCAN_UNLOCK_ASSERT(_lhw)		\
     mtx_assert(&(_lhw)->scan_mtx, MA_NOTOWNED)
 
+#define	LKPI_80211_LHW_TXQ_LOCK_INIT(_lhw)		\
+    mtx_init(&(_lhw)->txq_mtx, "lhw-txq", NULL, MTX_DEF | MTX_RECURSE);
+#define	LKPI_80211_LHW_TXQ_LOCK_DESTROY(_lhw)		\
+    mtx_destroy(&(_lhw)->txq_mtx);
+#define	LKPI_80211_LHW_TXQ_LOCK(_lhw)			\
+    mtx_lock(&(_lhw)->txq_mtx)
+#define	LKPI_80211_LHW_TXQ_UNLOCK(_lhw)			\
+    mtx_unlock(&(_lhw)->txq_mtx)
+#define	LKPI_80211_LHW_TXQ_LOCK_ASSERT(_lhw)		\
+    mtx_assert(&(_lhw)->txq_mtx, MA_OWNED)
+#define	LKPI_80211_LHW_TXQ_UNLOCK_ASSERT(_lhw)		\
+    mtx_assert(&(_lhw)->txq_mtx, MA_NOTOWNED)
+
 #define	LKPI_80211_LHW_LVIF_LOCK(_lhw)	sx_xlock(&(_lhw)->lvif_sx)
 #define	LKPI_80211_LHW_LVIF_UNLOCK(_lhw) sx_xunlock(&(_lhw)->lvif_sx)
 
@@ -295,6 +310,18 @@ struct lkpi_wiphy {
 #define	LKPI_80211_LSTA_LOCK(_lsta)	mtx_lock(&(_lsta)->txq_mtx)
 #define	LKPI_80211_LSTA_UNLOCK(_lsta)	mtx_unlock(&(_lsta)->txq_mtx)
 
+#define	LKPI_80211_LTXQ_LOCK_INIT(_ltxq)		\
+    mtx_init(&(_ltxq)->ltxq_mtx, "ltxq", NULL, MTX_DEF);
+#define	LKPI_80211_LTXQ_LOCK_DESTROY(_ltxq)		\
+    mtx_destroy(&(_ltxq)->ltxq_mtx);
+#define	LKPI_80211_LTXQ_LOCK(_ltxq)			\
+    mtx_lock(&(_ltxq)->ltxq_mtx)
+#define	LKPI_80211_LTXQ_UNLOCK(_ltxq)			\
+    mtx_unlock(&(_ltxq)->ltxq_mtx)
+#define	LKPI_80211_LTXQ_LOCK_ASSERT(_ltxq)		\
+    mtx_assert(&(_ltxq)->ltxq_mtx, MA_OWNED)
+#define	LKPI_80211_LTXQ_UNLOCK_ASSERT(_ltxq)		\
+    mtx_assert(&(_ltxq)->ltxq_mtx, MA_NOTOWNED)
 
 int lkpi_80211_mo_start(struct ieee80211_hw *);
 void lkpi_80211_mo_stop(struct ieee80211_hw *);