From b4df47081b67bce9dcb7b84b551588c7402a330a Mon Sep 17 00:00:00 2001
From: Gertjan van Wingerde <gwingerde@gmail.com>
Date: Thu, 13 May 2010 13:16:36 +0200
Subject: rt2x00: Move rt2x00debug_dump_frame declaration to rt2x00.h.

This allows rt2x00debug_dump_frame to be used from everywhere.

This is preparation for beacon writing clean ups.

Signed-off-by: Gertjan van Wingerde <gwingerde@gmail.com>
Acked-by: Ivo van Doorn <IvDoorn@gmail.com>
Signed-off-by: John W. Linville <linville@tuxdriver.com>
---
 drivers/net/wireless/rt2x00/rt2x00lib.h | 10 ----------
 1 file changed, 10 deletions(-)

(limited to 'drivers/net/wireless/rt2x00/rt2x00lib.h')

diff --git a/drivers/net/wireless/rt2x00/rt2x00lib.h b/drivers/net/wireless/rt2x00/rt2x00lib.h
index be2e37fb407..0ca40e1fe69 100644
--- a/drivers/net/wireless/rt2x00/rt2x00lib.h
+++ b/drivers/net/wireless/rt2x00/rt2x00lib.h
@@ -27,8 +27,6 @@
 #ifndef RT2X00LIB_H
 #define RT2X00LIB_H
 
-#include "rt2x00dump.h"
-
 /*
  * Interval defines
  */
@@ -296,8 +294,6 @@ static inline void rt2x00lib_free_firmware(struct rt2x00_dev *rt2x00dev)
 #ifdef CONFIG_RT2X00_LIB_DEBUGFS
 void rt2x00debug_register(struct rt2x00_dev *rt2x00dev);
 void rt2x00debug_deregister(struct rt2x00_dev *rt2x00dev);
-void rt2x00debug_dump_frame(struct rt2x00_dev *rt2x00dev,
-			    enum rt2x00_dump_type type, struct sk_buff *skb);
 void rt2x00debug_update_crypto(struct rt2x00_dev *rt2x00dev,
 			       struct rxdone_entry_desc *rxdesc);
 #else
@@ -309,12 +305,6 @@ static inline void rt2x00debug_deregister(struct rt2x00_dev *rt2x00dev)
 {
 }
 
-static inline void rt2x00debug_dump_frame(struct rt2x00_dev *rt2x00dev,
-					  enum rt2x00_dump_type type,
-					  struct sk_buff *skb)
-{
-}
-
 static inline void rt2x00debug_update_crypto(struct rt2x00_dev *rt2x00dev,
 					     struct rxdone_entry_desc *rxdesc)
 {
-- 
cgit v1.2.3-70-g09d2


From 0b8004aa12d13ec750d102ba4082a95f0107c649 Mon Sep 17 00:00:00 2001
From: Gertjan van Wingerde <gwingerde@gmail.com>
Date: Thu, 3 Jun 2010 10:51:45 +0200
Subject: rt2x00: Properly reserve room for descriptors in skbs.

Instead of fiddling with the skb->data pointer and thereby risking
out of bounds accesses, properly reserve the space needed in an
skb for descriptors.

Signed-off-by: Gertjan van Wingerde <gwingerde@gmail.com>
Acked-by: Ivo van Doorn <IvDoorn@gmail.com>
Signed-off-by: Ivo van Doorn <IvDoorn@gmail.com>
---
 drivers/net/wireless/rt2x00/rt2400pci.c   |  2 +-
 drivers/net/wireless/rt2x00/rt2500pci.c   |  2 +-
 drivers/net/wireless/rt2x00/rt2500usb.c   | 14 ++++++-----
 drivers/net/wireless/rt2x00/rt2800lib.c   |  3 +--
 drivers/net/wireless/rt2x00/rt2800lib.h   |  2 +-
 drivers/net/wireless/rt2x00/rt2800pci.c   | 23 +++++++++---------
 drivers/net/wireless/rt2x00/rt2800usb.c   | 22 ++++++++++-------
 drivers/net/wireless/rt2x00/rt2x00.h      |  7 ++++++
 drivers/net/wireless/rt2x00/rt2x00dev.c   |  5 ----
 drivers/net/wireless/rt2x00/rt2x00lib.h   |  7 ------
 drivers/net/wireless/rt2x00/rt2x00pci.c   | 40 +++++++++++++++++++++++++++++++
 drivers/net/wireless/rt2x00/rt2x00pci.h   |  8 +++++++
 drivers/net/wireless/rt2x00/rt2x00queue.c | 24 ++-----------------
 drivers/net/wireless/rt2x00/rt2x00usb.c   | 11 ++++-----
 drivers/net/wireless/rt2x00/rt61pci.c     |  4 ++--
 drivers/net/wireless/rt2x00/rt73usb.c     | 14 ++++++-----
 16 files changed, 109 insertions(+), 79 deletions(-)

(limited to 'drivers/net/wireless/rt2x00/rt2x00lib.h')

diff --git a/drivers/net/wireless/rt2x00/rt2400pci.c b/drivers/net/wireless/rt2x00/rt2400pci.c
index 1e543ac2f86..1eb882e15fb 100644
--- a/drivers/net/wireless/rt2x00/rt2400pci.c
+++ b/drivers/net/wireless/rt2x00/rt2400pci.c
@@ -1229,7 +1229,7 @@ static void rt2400pci_txdone(struct rt2x00_dev *rt2x00dev,
 		}
 		txdesc.retry = rt2x00_get_field32(word, TXD_W0_RETRY_COUNT);
 
-		rt2x00lib_txdone(entry, &txdesc);
+		rt2x00pci_txdone(entry, &txdesc);
 	}
 }
 
diff --git a/drivers/net/wireless/rt2x00/rt2500pci.c b/drivers/net/wireless/rt2x00/rt2500pci.c
index 1582cabd3a1..a29cb212f89 100644
--- a/drivers/net/wireless/rt2x00/rt2500pci.c
+++ b/drivers/net/wireless/rt2x00/rt2500pci.c
@@ -1365,7 +1365,7 @@ static void rt2500pci_txdone(struct rt2x00_dev *rt2x00dev,
 		}
 		txdesc.retry = rt2x00_get_field32(word, TXD_W0_RETRY_COUNT);
 
-		rt2x00lib_txdone(entry, &txdesc);
+		rt2x00pci_txdone(entry, &txdesc);
 	}
 }
 
diff --git a/drivers/net/wireless/rt2x00/rt2500usb.c b/drivers/net/wireless/rt2x00/rt2500usb.c
index d19f29a5352..9dab1dccdaf 100644
--- a/drivers/net/wireless/rt2x00/rt2500usb.c
+++ b/drivers/net/wireless/rt2x00/rt2500usb.c
@@ -1034,7 +1034,7 @@ static void rt2500usb_write_tx_desc(struct rt2x00_dev *rt2x00dev,
 				    struct txentry_desc *txdesc)
 {
 	struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
-	__le32 *txd = (__le32 *)(skb->data - TXD_DESC_SIZE);
+	__le32 *txd = (__le32 *) skb->data;
 	u32 word;
 
 	/*
@@ -1080,6 +1080,7 @@ static void rt2500usb_write_tx_desc(struct rt2x00_dev *rt2x00dev,
 	/*
 	 * Register descriptor details in skb frame descriptor.
 	 */
+	skbdesc->flags |= SKBDESC_DESC_IN_SKB;
 	skbdesc->desc = txd;
 	skbdesc->desc_len = TXD_DESC_SIZE;
 }
@@ -1107,6 +1108,12 @@ static void rt2500usb_write_beacon(struct queue_entry *entry,
 	rt2x00_set_field16(&reg, TXRX_CSR19_BEACON_GEN, 0);
 	rt2500usb_register_write(rt2x00dev, TXRX_CSR19, reg);
 
+	/*
+	 * Add space for the descriptor in front of the skb.
+	 */
+	skb_push(entry->skb, TXD_DESC_SIZE);
+	memset(entry->skb->data, 0, TXD_DESC_SIZE);
+
 	/*
 	 * Write the TX descriptor for the beacon.
 	 */
@@ -1117,11 +1124,6 @@ static void rt2500usb_write_beacon(struct queue_entry *entry,
 	 */
 	rt2x00debug_dump_frame(rt2x00dev, DUMP_FRAME_BEACON, entry->skb);
 
-	/*
-	 * Take the descriptor in front of the skb into account.
-	 */
-	skb_push(entry->skb, TXD_DESC_SIZE);
-
 	/*
 	 * USB devices cannot blindly pass the skb->len as the
 	 * length of the data to usb_fill_bulk_urb. Pass the skb
diff --git a/drivers/net/wireless/rt2x00/rt2800lib.c b/drivers/net/wireless/rt2x00/rt2800lib.c
index db4250d1c8b..3258301aa29 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.c
+++ b/drivers/net/wireless/rt2x00/rt2800lib.c
@@ -282,9 +282,8 @@ int rt2800_wait_wpdma_ready(struct rt2x00_dev *rt2x00dev)
 }
 EXPORT_SYMBOL_GPL(rt2800_wait_wpdma_ready);
 
-void rt2800_write_txwi(struct sk_buff *skb, struct txentry_desc *txdesc)
+void rt2800_write_txwi(__le32 *txwi, struct txentry_desc *txdesc)
 {
-	__le32 *txwi = (__le32 *)(skb->data - TXWI_DESC_SIZE);
 	u32 word;
 
 	/*
diff --git a/drivers/net/wireless/rt2x00/rt2800lib.h b/drivers/net/wireless/rt2x00/rt2800lib.h
index 94de999e229..0f0a13c61e6 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.h
+++ b/drivers/net/wireless/rt2x00/rt2800lib.h
@@ -111,7 +111,7 @@ void rt2800_mcu_request(struct rt2x00_dev *rt2x00dev,
 			const u8 command, const u8 token,
 			const u8 arg0, const u8 arg1);
 
-void rt2800_write_txwi(struct sk_buff *skb, struct txentry_desc *txdesc);
+void rt2800_write_txwi(__le32 *txwi, struct txentry_desc *txdesc);
 void rt2800_process_rxwi(struct sk_buff *skb, struct rxdone_entry_desc *txdesc);
 
 extern const struct rt2x00debug rt2800_rt2x00debug;
diff --git a/drivers/net/wireless/rt2x00/rt2800pci.c b/drivers/net/wireless/rt2x00/rt2800pci.c
index 72e4f29a2fc..db61a78e32b 100644
--- a/drivers/net/wireless/rt2x00/rt2800pci.c
+++ b/drivers/net/wireless/rt2x00/rt2800pci.c
@@ -616,7 +616,7 @@ static int rt2800pci_set_device_state(struct rt2x00_dev *rt2x00dev,
 static void rt2800pci_write_tx_datadesc(struct queue_entry* entry,
 					 struct txentry_desc *txdesc)
 {
-	rt2800_write_txwi(entry->skb, txdesc);
+	rt2800_write_txwi((__le32 *) entry->skb->data, txdesc);
 }
 
 
@@ -692,27 +692,29 @@ static void rt2800pci_write_beacon(struct queue_entry *entry,
 	rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_GEN, 0);
 	rt2800_register_write(rt2x00dev, BCN_TIME_CFG, reg);
 
+	/*
+	 * Add space for the TXWI in front of the skb.
+	 */
+	skb_push(entry->skb, TXWI_DESC_SIZE);
+	memset(entry->skb, 0, TXWI_DESC_SIZE);
+
 	/*
 	 * Register descriptor details in skb frame descriptor.
 	 */
-	skbdesc->desc = entry->skb->data - TXWI_DESC_SIZE;
+	skbdesc->flags |= SKBDESC_DESC_IN_SKB;
+	skbdesc->desc = entry->skb->data;
 	skbdesc->desc_len = TXWI_DESC_SIZE;
 
 	/*
 	 * Add the TXWI for the beacon to the skb.
 	 */
-	rt2800_write_txwi(entry->skb, txdesc);
+	rt2800_write_txwi((__le32 *)entry->skb->data, txdesc);
 
 	/*
 	 * Dump beacon to userspace through debugfs.
 	 */
 	rt2x00debug_dump_frame(rt2x00dev, DUMP_FRAME_BEACON, entry->skb);
 
-	/*
-	 * Adjust skb to take TXWI into account.
-	 */
-	skb_push(entry->skb, TXWI_DESC_SIZE);
-
 	/*
 	 * Write entire beacon with TXWI to register.
 	 */
@@ -888,8 +890,7 @@ static void rt2800pci_txdone(struct rt2x00_dev *rt2x00dev)
 
 		/* Check if we got a match by looking at WCID/ACK/PID
 		 * fields */
-		txwi = (__le32 *)(entry->skb->data -
-				  rt2x00dev->ops->extra_tx_headroom);
+		txwi = (__le32 *) entry->skb->data;
 
 		rt2x00_desc_read(txwi, 1, &word);
 		tx_wcid = rt2x00_get_field32(word, TXWI_W1_WIRELESS_CLI_ID);
@@ -934,7 +935,7 @@ static void rt2800pci_txdone(struct rt2x00_dev *rt2x00dev)
 		__set_bit(TXDONE_FALLBACK, &txdesc.flags);
 
 
-		rt2x00lib_txdone(entry, &txdesc);
+		rt2x00pci_txdone(entry, &txdesc);
 	}
 }
 
diff --git a/drivers/net/wireless/rt2x00/rt2800usb.c b/drivers/net/wireless/rt2x00/rt2800usb.c
index d0d8060040b..ee407f13875 100644
--- a/drivers/net/wireless/rt2x00/rt2800usb.c
+++ b/drivers/net/wireless/rt2x00/rt2800usb.c
@@ -400,13 +400,14 @@ static void rt2800usb_write_tx_desc(struct rt2x00_dev *rt2x00dev,
 				    struct txentry_desc *txdesc)
 {
 	struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
-	__le32 *txi = (__le32 *)(skb->data - TXWI_DESC_SIZE - TXINFO_DESC_SIZE);
+	__le32 *txi = (__le32 *) skb->data;
+	__le32 *txwi = (__le32 *) (skb->data + TXINFO_DESC_SIZE);
 	u32 word;
 
 	/*
 	 * Initialize TXWI descriptor
 	 */
-	rt2800_write_txwi(skb, txdesc);
+	rt2800_write_txwi(txwi, txdesc);
 
 	/*
 	 * Initialize TXINFO descriptor
@@ -426,6 +427,7 @@ static void rt2800usb_write_tx_desc(struct rt2x00_dev *rt2x00dev,
 	/*
 	 * Register descriptor details in skb frame descriptor.
 	 */
+	skbdesc->flags |= SKBDESC_DESC_IN_SKB;
 	skbdesc->desc = txi;
 	skbdesc->desc_len = TXINFO_DESC_SIZE + TXWI_DESC_SIZE;
 }
@@ -449,27 +451,29 @@ static void rt2800usb_write_beacon(struct queue_entry *entry,
 	rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_GEN, 0);
 	rt2800_register_write(rt2x00dev, BCN_TIME_CFG, reg);
 
+	/*
+	 * Add space for the TXWI in front of the skb.
+	 */
+	skb_push(entry->skb, TXWI_DESC_SIZE);
+	memset(entry->skb, 0, TXWI_DESC_SIZE);
+
 	/*
 	 * Register descriptor details in skb frame descriptor.
 	 */
-	skbdesc->desc = entry->skb->data - TXWI_DESC_SIZE;
+	skbdesc->flags |= SKBDESC_DESC_IN_SKB;
+	skbdesc->desc = entry->skb->data;
 	skbdesc->desc_len = TXWI_DESC_SIZE;
 
 	/*
 	 * Add the TXWI for the beacon to the skb.
 	 */
-	rt2800_write_txwi(entry->skb, txdesc);
+	rt2800_write_txwi((__le32 *) entry->skb->data, txdesc);
 
 	/*
 	 * Dump beacon to userspace through debugfs.
 	 */
 	rt2x00debug_dump_frame(rt2x00dev, DUMP_FRAME_BEACON, entry->skb);
 
-	/*
-	 * Adjust skb to take TXWI into account.
-	 */
-	skb_push(entry->skb, TXWI_DESC_SIZE);
-
 	/*
 	 * Write entire beacon with descriptor to register.
 	 */
diff --git a/drivers/net/wireless/rt2x00/rt2x00.h b/drivers/net/wireless/rt2x00/rt2x00.h
index 811844be005..889a372367f 100644
--- a/drivers/net/wireless/rt2x00/rt2x00.h
+++ b/drivers/net/wireless/rt2x00/rt2x00.h
@@ -1001,6 +1001,13 @@ static inline bool rt2x00_is_soc(struct rt2x00_dev *rt2x00dev)
  */
 void rt2x00queue_map_txskb(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb);
 
+/**
+ * rt2x00queue_unmap_skb - Unmap a skb from DMA.
+ * @rt2x00dev: Pointer to &struct rt2x00_dev.
+ * @skb: The skb to unmap.
+ */
+void rt2x00queue_unmap_skb(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb);
+
 /**
  * rt2x00queue_get_queue - Convert queue index to queue pointer
  * @rt2x00dev: Pointer to &struct rt2x00_dev.
diff --git a/drivers/net/wireless/rt2x00/rt2x00dev.c b/drivers/net/wireless/rt2x00/rt2x00dev.c
index 2ed32e02a06..0b8efe8e678 100644
--- a/drivers/net/wireless/rt2x00/rt2x00dev.c
+++ b/drivers/net/wireless/rt2x00/rt2x00dev.c
@@ -210,11 +210,6 @@ void rt2x00lib_txdone(struct queue_entry *entry,
 	unsigned int i;
 	bool success;
 
-	/*
-	 * Unmap the skb.
-	 */
-	rt2x00queue_unmap_skb(rt2x00dev, entry->skb);
-
 	/*
 	 * Remove L2 padding which was added during
 	 */
diff --git a/drivers/net/wireless/rt2x00/rt2x00lib.h b/drivers/net/wireless/rt2x00/rt2x00lib.h
index 0ca40e1fe69..822affc9b4c 100644
--- a/drivers/net/wireless/rt2x00/rt2x00lib.h
+++ b/drivers/net/wireless/rt2x00/rt2x00lib.h
@@ -104,13 +104,6 @@ void rt2x00lib_config(struct rt2x00_dev *rt2x00dev,
 struct sk_buff *rt2x00queue_alloc_rxskb(struct rt2x00_dev *rt2x00dev,
 					struct queue_entry *entry);
 
-/**
- * rt2x00queue_unmap_skb - Unmap a skb from DMA.
- * @rt2x00dev: Pointer to &struct rt2x00_dev.
- * @skb: The skb to unmap.
- */
-void rt2x00queue_unmap_skb(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb);
-
 /**
  * rt2x00queue_free_skb - free a skb
  * @rt2x00dev: Pointer to &struct rt2x00_dev.
diff --git a/drivers/net/wireless/rt2x00/rt2x00pci.c b/drivers/net/wireless/rt2x00/rt2x00pci.c
index 494b960e811..d583ee070b4 100644
--- a/drivers/net/wireless/rt2x00/rt2x00pci.c
+++ b/drivers/net/wireless/rt2x00/rt2x00pci.c
@@ -81,12 +81,24 @@ int rt2x00pci_write_tx_data(struct queue_entry *entry,
 		return -EINVAL;
 	}
 
+	/*
+	 * Add the requested extra tx headroom in front of the skb.
+	 */
+	skb_push(entry->skb, rt2x00dev->ops->extra_tx_headroom);
+	memset(entry->skb->data, 0, rt2x00dev->ops->extra_tx_headroom);
+
 	/*
 	 * Call the driver's write_tx_datadesc function, if it exists.
 	 */
 	if (rt2x00dev->ops->lib->write_tx_datadesc)
 		rt2x00dev->ops->lib->write_tx_datadesc(entry, txdesc);
 
+	/*
+	 * Map the skb to DMA.
+	 */
+	if (test_bit(DRIVER_REQUIRE_DMA, &rt2x00dev->flags))
+		rt2x00queue_map_txskb(rt2x00dev, entry->skb);
+
 	return 0;
 }
 EXPORT_SYMBOL_GPL(rt2x00pci_write_tx_data);
@@ -94,6 +106,34 @@ EXPORT_SYMBOL_GPL(rt2x00pci_write_tx_data);
 /*
  * TX/RX data handlers.
  */
+void rt2x00pci_txdone(struct queue_entry *entry,
+		      struct txdone_entry_desc *txdesc)
+{
+	struct rt2x00_dev *rt2x00dev = entry->queue->rt2x00dev;
+	struct skb_frame_desc *skbdesc = get_skb_frame_desc(entry->skb);
+
+	/*
+	 * Unmap the skb.
+	 */
+	rt2x00queue_unmap_skb(rt2x00dev, entry->skb);
+
+	/*
+	 * Remove the extra tx headroom from the skb.
+	 */
+	skb_pull(entry->skb, rt2x00dev->ops->extra_tx_headroom);
+
+	/*
+	 * Signal that the TX descriptor is no longer in the skb.
+	 */
+	skbdesc->flags &= ~SKBDESC_DESC_IN_SKB;
+
+	/*
+	 * Pass on to rt2x00lib.
+	 */
+	rt2x00lib_txdone(entry, txdesc);
+}
+EXPORT_SYMBOL_GPL(rt2x00pci_txdone);
+
 void rt2x00pci_rxdone(struct rt2x00_dev *rt2x00dev)
 {
 	struct data_queue *queue = rt2x00dev->rx;
diff --git a/drivers/net/wireless/rt2x00/rt2x00pci.h b/drivers/net/wireless/rt2x00/rt2x00pci.h
index 51bcef3839c..00528b8a754 100644
--- a/drivers/net/wireless/rt2x00/rt2x00pci.h
+++ b/drivers/net/wireless/rt2x00/rt2x00pci.h
@@ -108,6 +108,14 @@ struct queue_entry_priv_pci {
 	dma_addr_t desc_dma;
 };
 
+/**
+ * rt2x00pci_txdone - Handle TX done events.
+ * @entry: The queue entry for which a TX done event was received.
+ * @txdesc: The TX done descriptor for the entry.
+ */
+void rt2x00pci_txdone(struct queue_entry *entry,
+		      struct txdone_entry_desc *txdesc);
+
 /**
  * rt2x00pci_rxdone - Handle RX done events
  * @rt2x00dev: Device pointer, see &struct rt2x00_dev.
diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.c b/drivers/net/wireless/rt2x00/rt2x00queue.c
index cf7bfe774e0..35858b178e8 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.c
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.c
@@ -100,21 +100,8 @@ void rt2x00queue_map_txskb(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb)
 {
 	struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
 
-	/*
-	 * If device has requested headroom, we should make sure that
-	 * is also mapped to the DMA so it can be used for transfering
-	 * additional descriptor information to the hardware.
-	 */
-	skb_push(skb, rt2x00dev->ops->extra_tx_headroom);
-
 	skbdesc->skb_dma =
 	    dma_map_single(rt2x00dev->dev, skb->data, skb->len, DMA_TO_DEVICE);
-
-	/*
-	 * Restore data pointer to original location again.
-	 */
-	skb_pull(skb, rt2x00dev->ops->extra_tx_headroom);
-
 	skbdesc->flags |= SKBDESC_DMA_MAPPED_TX;
 }
 EXPORT_SYMBOL_GPL(rt2x00queue_map_txskb);
@@ -130,16 +117,12 @@ void rt2x00queue_unmap_skb(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb)
 	}
 
 	if (skbdesc->flags & SKBDESC_DMA_MAPPED_TX) {
-		/*
-		 * Add headroom to the skb length, it has been removed
-		 * by the driver, but it was actually mapped to DMA.
-		 */
-		dma_unmap_single(rt2x00dev->dev, skbdesc->skb_dma,
-				 skb->len + rt2x00dev->ops->extra_tx_headroom,
+		dma_unmap_single(rt2x00dev->dev, skbdesc->skb_dma, skb->len,
 				 DMA_TO_DEVICE);
 		skbdesc->flags &= ~SKBDESC_DMA_MAPPED_TX;
 	}
 }
+EXPORT_SYMBOL_GPL(rt2x00queue_unmap_skb);
 
 void rt2x00queue_free_skb(struct rt2x00_dev *rt2x00dev, struct sk_buff *skb)
 {
@@ -534,9 +517,6 @@ int rt2x00queue_write_tx_frame(struct data_queue *queue, struct sk_buff *skb,
 		return -EIO;
 	}
 
-	if (test_bit(DRIVER_REQUIRE_DMA, &queue->rt2x00dev->flags))
-		rt2x00queue_map_txskb(queue->rt2x00dev, skb);
-
 	set_bit(ENTRY_DATA_PENDING, &entry->flags);
 
 	rt2x00queue_index_inc(queue, Q_INDEX);
diff --git a/drivers/net/wireless/rt2x00/rt2x00usb.c b/drivers/net/wireless/rt2x00/rt2x00usb.c
index 25cc376d388..5e123519f8c 100644
--- a/drivers/net/wireless/rt2x00/rt2x00usb.c
+++ b/drivers/net/wireless/rt2x00/rt2x00usb.c
@@ -197,6 +197,11 @@ static void rt2x00usb_interrupt_txdone(struct urb *urb)
 	    !test_bit(ENTRY_OWNER_DEVICE_DATA, &entry->flags))
 		return;
 
+	/*
+	 * Remove the descriptor from the front of the skb.
+	 */
+	skb_pull(entry->skb, entry->queue->desc_size);
+
 	/*
 	 * Obtain the status about this packet.
 	 * Note that when the status is 0 it does not mean the
@@ -242,12 +247,6 @@ int rt2x00usb_write_tx_data(struct queue_entry *entry,
 			  entry->skb->data, length,
 			  rt2x00usb_interrupt_txdone, entry);
 
-	/*
-	 * Make sure the skb->data pointer points to the frame, not the
-	 * descriptor.
-	 */
-	skb_pull(entry->skb, entry->queue->desc_size);
-
 	/*
 	 * Call the driver's write_tx_datadesc function, if it exists.
 	 */
diff --git a/drivers/net/wireless/rt2x00/rt61pci.c b/drivers/net/wireless/rt2x00/rt61pci.c
index a7205c1711d..243df08ae91 100644
--- a/drivers/net/wireless/rt2x00/rt61pci.c
+++ b/drivers/net/wireless/rt2x00/rt61pci.c
@@ -2110,7 +2110,7 @@ static void rt61pci_txdone(struct rt2x00_dev *rt2x00dev)
 			__set_bit(TXDONE_UNKNOWN, &txdesc.flags);
 			txdesc.retry = 0;
 
-			rt2x00lib_txdone(entry_done, &txdesc);
+			rt2x00pci_txdone(entry_done, &txdesc);
 			entry_done = rt2x00queue_get_entry(queue, Q_INDEX_DONE);
 		}
 
@@ -2130,7 +2130,7 @@ static void rt61pci_txdone(struct rt2x00_dev *rt2x00dev)
 		}
 		txdesc.retry = rt2x00_get_field32(reg, STA_CSR4_RETRY_COUNT);
 
-		rt2x00lib_txdone(entry, &txdesc);
+		rt2x00pci_txdone(entry, &txdesc);
 	}
 }
 
diff --git a/drivers/net/wireless/rt2x00/rt73usb.c b/drivers/net/wireless/rt2x00/rt73usb.c
index bd9a53e5fd9..4ab38c3641c 100644
--- a/drivers/net/wireless/rt2x00/rt73usb.c
+++ b/drivers/net/wireless/rt2x00/rt73usb.c
@@ -1442,7 +1442,7 @@ static void rt73usb_write_tx_desc(struct rt2x00_dev *rt2x00dev,
 				  struct txentry_desc *txdesc)
 {
 	struct skb_frame_desc *skbdesc = get_skb_frame_desc(skb);
-	__le32 *txd = (__le32 *)(skb->data - TXD_DESC_SIZE);
+	__le32 *txd = (__le32 *) skb->data;
 	u32 word;
 
 	/*
@@ -1505,6 +1505,7 @@ static void rt73usb_write_tx_desc(struct rt2x00_dev *rt2x00dev,
 	/*
 	 * Register descriptor details in skb frame descriptor.
 	 */
+	skbdesc->flags |= SKBDESC_DESC_IN_SKB;
 	skbdesc->desc = txd;
 	skbdesc->desc_len = TXD_DESC_SIZE;
 }
@@ -1527,6 +1528,12 @@ static void rt73usb_write_beacon(struct queue_entry *entry,
 	rt2x00_set_field32(&reg, TXRX_CSR9_BEACON_GEN, 0);
 	rt2x00usb_register_write(rt2x00dev, TXRX_CSR9, reg);
 
+	/*
+	 * Add space for the descriptor in front of the skb.
+	 */
+	skb_push(entry->skb, TXD_DESC_SIZE);
+	memset(entry->skb->data, 0, TXD_DESC_SIZE);
+
 	/*
 	 * Write the TX descriptor for the beacon.
 	 */
@@ -1537,11 +1544,6 @@ static void rt73usb_write_beacon(struct queue_entry *entry,
 	 */
 	rt2x00debug_dump_frame(rt2x00dev, DUMP_FRAME_BEACON, entry->skb);
 
-	/*
-	 * Take the descriptor in front of the skb into account.
-	 */
-	skb_push(entry->skb, TXD_DESC_SIZE);
-
 	/*
 	 * Write entire beacon with descriptor to register.
 	 */
-- 
cgit v1.2.3-70-g09d2


From 06443e46c65915d74b03fe1de10c00748e4706ee Mon Sep 17 00:00:00 2001
From: Gertjan van Wingerde <gwingerde@gmail.com>
Date: Thu, 3 Jun 2010 10:52:08 +0200
Subject: rt2x00: Fix HT40 operation in rt2800.

Closer inspection of the legacy Ralink driver reveals that in case of HT40+
or HT40- we must adjust the frequency settings that we program to the device.
Implement the same adjustment in the rt2x00 code.

With this HT40 seems to work for all devices supported by rt2800pci and
rt2800usb.

Signed-off-by: Gertjan van Wingerde <gwingerde@gmail.com>
Signed-off-by: Ivo van Doorn <IvDoorn@gmail.com>
---
 drivers/net/wireless/rt2x00/rt2800lib.c    |  5 +----
 drivers/net/wireless/rt2x00/rt2x00config.c | 12 ++++++++----
 drivers/net/wireless/rt2x00/rt2x00ht.c     | 28 ++++++++++++++++++++++++++++
 drivers/net/wireless/rt2x00/rt2x00lib.h    |  9 +++++++++
 4 files changed, 46 insertions(+), 8 deletions(-)

(limited to 'drivers/net/wireless/rt2x00/rt2x00lib.h')

diff --git a/drivers/net/wireless/rt2x00/rt2800lib.c b/drivers/net/wireless/rt2x00/rt2800lib.c
index 9aa48011717..bebbda5ffaa 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.c
+++ b/drivers/net/wireless/rt2x00/rt2800lib.c
@@ -2531,11 +2531,8 @@ int rt2800_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 	else
 		spec->ht.ht_supported = false;
 
-	/*
-	 * Don't set IEEE80211_HT_CAP_SUP_WIDTH_20_40 for now as it causes
-	 * reception problems with HT40 capable 11n APs
-	 */
 	spec->ht.cap =
+	    IEEE80211_HT_CAP_SUP_WIDTH_20_40 |
 	    IEEE80211_HT_CAP_GRN_FLD |
 	    IEEE80211_HT_CAP_SGI_20 |
 	    IEEE80211_HT_CAP_SGI_40 |
diff --git a/drivers/net/wireless/rt2x00/rt2x00config.c b/drivers/net/wireless/rt2x00/rt2x00config.c
index 098315a271c..8dbd634dae2 100644
--- a/drivers/net/wireless/rt2x00/rt2x00config.c
+++ b/drivers/net/wireless/rt2x00/rt2x00config.c
@@ -170,23 +170,27 @@ void rt2x00lib_config(struct rt2x00_dev *rt2x00dev,
 		      unsigned int ieee80211_flags)
 {
 	struct rt2x00lib_conf libconf;
+	u16 hw_value;
 
 	memset(&libconf, 0, sizeof(libconf));
 
 	libconf.conf = conf;
 
 	if (ieee80211_flags & IEEE80211_CONF_CHANGE_CHANNEL) {
-		if (conf_is_ht40(conf))
+		if (conf_is_ht40(conf)) {
 			__set_bit(CONFIG_CHANNEL_HT40, &rt2x00dev->flags);
-		else
+			hw_value = rt2x00ht_center_channel(rt2x00dev, conf);
+		} else {
 			__clear_bit(CONFIG_CHANNEL_HT40, &rt2x00dev->flags);
+			hw_value = conf->channel->hw_value;
+		}
 
 		memcpy(&libconf.rf,
-		       &rt2x00dev->spec.channels[conf->channel->hw_value],
+		       &rt2x00dev->spec.channels[hw_value],
 		       sizeof(libconf.rf));
 
 		memcpy(&libconf.channel,
-		       &rt2x00dev->spec.channels_info[conf->channel->hw_value],
+		       &rt2x00dev->spec.channels_info[hw_value],
 		       sizeof(libconf.channel));
 	}
 
diff --git a/drivers/net/wireless/rt2x00/rt2x00ht.c b/drivers/net/wireless/rt2x00/rt2x00ht.c
index 5a407602ce3..c4b749da430 100644
--- a/drivers/net/wireless/rt2x00/rt2x00ht.c
+++ b/drivers/net/wireless/rt2x00/rt2x00ht.c
@@ -84,3 +84,31 @@ void rt2x00ht_create_tx_descriptor(struct queue_entry *entry,
 	else
 		txdesc->txop = TXOP_HTTXOP;
 }
+
+u16 rt2x00ht_center_channel(struct rt2x00_dev *rt2x00dev,
+			    struct ieee80211_conf *conf)
+{
+	struct hw_mode_spec *spec = &rt2x00dev->spec;
+	int center_channel;
+	u16 i;
+
+	/*
+	 * Initialize center channel to current channel.
+	 */
+	center_channel = spec->channels[conf->channel->hw_value].channel;
+
+	/*
+	 * Adjust center channel to HT40+ and HT40- operation.
+	 */
+	if (conf_is_ht40_plus(conf))
+		center_channel += 2;
+	else if (conf_is_ht40_minus(conf))
+		center_channel -= (center_channel == 14) ? 1 : 2;
+
+	for (i = 0; i < spec->num_channels; i++)
+		if (spec->channels[i].channel == center_channel)
+			return i;
+
+	WARN_ON(1);
+	return conf->channel->hw_value;
+}
diff --git a/drivers/net/wireless/rt2x00/rt2x00lib.h b/drivers/net/wireless/rt2x00/rt2x00lib.h
index 822affc9b4c..ed27de1de57 100644
--- a/drivers/net/wireless/rt2x00/rt2x00lib.h
+++ b/drivers/net/wireless/rt2x00/rt2x00lib.h
@@ -367,12 +367,21 @@ static inline void rt2x00crypto_rx_insert_iv(struct sk_buff *skb,
 void rt2x00ht_create_tx_descriptor(struct queue_entry *entry,
 				   struct txentry_desc *txdesc,
 				   const struct rt2x00_rate *hwrate);
+
+u16 rt2x00ht_center_channel(struct rt2x00_dev *rt2x00dev,
+			    struct ieee80211_conf *conf);
 #else
 static inline void rt2x00ht_create_tx_descriptor(struct queue_entry *entry,
 						 struct txentry_desc *txdesc,
 						 const struct rt2x00_rate *hwrate)
 {
 }
+
+static inline u16 rt2x00ht_center_channel(struct rt2x00_dev *rt2x00dev,
+					  struct ieee80211_conf *conf)
+{
+	return conf->channel->hw_value;
+}
 #endif /* CONFIG_RT2X00_LIB_HT */
 
 /*
-- 
cgit v1.2.3-70-g09d2


From c965c74bbc650e5466d2f3e32bd28112ebcdd00c Mon Sep 17 00:00:00 2001
From: Ivo van Doorn <ivdoorn@gmail.com>
Date: Sun, 11 Jul 2010 12:25:46 +0200
Subject: rt2x00: Implement watchdog monitoring

Implement watchdog monitoring for USB devices (PCI support can
be added later). This will determine if URBs being uploaded to
the hardware are actually returning. Both rt2500usb and rt2800usb
have shown that URBs being uploaded can remain hanging without
being released by the hardware.
By using this watchdog, a queue can be reset when this occurs.
For rt2800usb it has been tested that the connection is preserved
even though this interruption.

Signed-off-by: Ivo van Doorn <IvDoorn@gmail.com>
Signed-off-by: John W. Linville <linville@tuxdriver.com>
---
 drivers/net/wireless/rt2x00/rt2500usb.c   |  2 ++
 drivers/net/wireless/rt2x00/rt2800usb.c   |  2 ++
 drivers/net/wireless/rt2x00/rt2x00.h      |  7 +++++
 drivers/net/wireless/rt2x00/rt2x00dev.c   | 10 +++++++
 drivers/net/wireless/rt2x00/rt2x00lib.h   | 26 ++++++++++++++--
 drivers/net/wireless/rt2x00/rt2x00link.c  | 38 +++++++++++++++++++++++
 drivers/net/wireless/rt2x00/rt2x00queue.c |  4 +++
 drivers/net/wireless/rt2x00/rt2x00queue.h | 11 +++++++
 drivers/net/wireless/rt2x00/rt2x00usb.c   | 50 +++++++++++++++++++++++++++++++
 drivers/net/wireless/rt2x00/rt2x00usb.h   | 10 +++++++
 drivers/net/wireless/rt2x00/rt73usb.c     |  2 ++
 11 files changed, 159 insertions(+), 3 deletions(-)

(limited to 'drivers/net/wireless/rt2x00/rt2x00lib.h')

diff --git a/drivers/net/wireless/rt2x00/rt2500usb.c b/drivers/net/wireless/rt2x00/rt2500usb.c
index 0b7888d43f3..009323e6c20 100644
--- a/drivers/net/wireless/rt2x00/rt2500usb.c
+++ b/drivers/net/wireless/rt2x00/rt2500usb.c
@@ -1736,6 +1736,7 @@ static int rt2500usb_probe_hw(struct rt2x00_dev *rt2x00dev)
 		__set_bit(CONFIG_SUPPORT_HW_CRYPTO, &rt2x00dev->flags);
 		__set_bit(DRIVER_REQUIRE_COPY_IV, &rt2x00dev->flags);
 	}
+	__set_bit(DRIVER_SUPPORT_WATCHDOG, &rt2x00dev->flags);
 
 	/*
 	 * Set the rssi offset.
@@ -1772,6 +1773,7 @@ static const struct rt2x00lib_ops rt2500usb_rt2x00_ops = {
 	.rfkill_poll		= rt2500usb_rfkill_poll,
 	.link_stats		= rt2500usb_link_stats,
 	.reset_tuner		= rt2500usb_reset_tuner,
+	.watchdog		= rt2x00usb_watchdog,
 	.write_tx_desc		= rt2500usb_write_tx_desc,
 	.write_beacon		= rt2500usb_write_beacon,
 	.get_tx_data_len	= rt2500usb_get_tx_data_len,
diff --git a/drivers/net/wireless/rt2x00/rt2800usb.c b/drivers/net/wireless/rt2x00/rt2800usb.c
index e4ab4db66ca..5aa7563155c 100644
--- a/drivers/net/wireless/rt2x00/rt2800usb.c
+++ b/drivers/net/wireless/rt2x00/rt2800usb.c
@@ -633,6 +633,7 @@ static int rt2800usb_probe_hw(struct rt2x00_dev *rt2x00dev)
 	if (!modparam_nohwcrypt)
 		__set_bit(CONFIG_SUPPORT_HW_CRYPTO, &rt2x00dev->flags);
 	__set_bit(DRIVER_SUPPORT_LINK_TUNING, &rt2x00dev->flags);
+	__set_bit(DRIVER_SUPPORT_WATCHDOG, &rt2x00dev->flags);
 
 	/*
 	 * Set the rssi offset.
@@ -655,6 +656,7 @@ static const struct rt2x00lib_ops rt2800usb_rt2x00_ops = {
 	.link_stats		= rt2800_link_stats,
 	.reset_tuner		= rt2800_reset_tuner,
 	.link_tuner		= rt2800_link_tuner,
+	.watchdog		= rt2x00usb_watchdog,
 	.write_tx_desc		= rt2800usb_write_tx_desc,
 	.write_tx_data		= rt2800usb_write_tx_data,
 	.write_beacon		= rt2800_write_beacon,
diff --git a/drivers/net/wireless/rt2x00/rt2x00.h b/drivers/net/wireless/rt2x00/rt2x00.h
index bf5e3f37e70..97b6261fee4 100644
--- a/drivers/net/wireless/rt2x00/rt2x00.h
+++ b/drivers/net/wireless/rt2x00/rt2x00.h
@@ -332,6 +332,11 @@ struct link {
 	 * Work structure for scheduling periodic link tuning.
 	 */
 	struct delayed_work work;
+
+	/*
+	 * Work structure for scheduling periodic watchdog monitoring.
+	 */
+	struct delayed_work watchdog_work;
 };
 
 /*
@@ -543,6 +548,7 @@ struct rt2x00lib_ops {
 			     struct link_qual *qual);
 	void (*link_tuner) (struct rt2x00_dev *rt2x00dev,
 			    struct link_qual *qual, const u32 count);
+	void (*watchdog) (struct rt2x00_dev *rt2x00dev);
 
 	/*
 	 * TX control handlers
@@ -648,6 +654,7 @@ enum rt2x00_flags {
 	DRIVER_SUPPORT_CONTROL_FILTERS,
 	DRIVER_SUPPORT_CONTROL_FILTER_PSPOLL,
 	DRIVER_SUPPORT_LINK_TUNING,
+	DRIVER_SUPPORT_WATCHDOG,
 
 	/*
 	 * Driver configuration
diff --git a/drivers/net/wireless/rt2x00/rt2x00dev.c b/drivers/net/wireless/rt2x00/rt2x00dev.c
index 12ee7bdedd0..0906e14b347 100644
--- a/drivers/net/wireless/rt2x00/rt2x00dev.c
+++ b/drivers/net/wireless/rt2x00/rt2x00dev.c
@@ -69,6 +69,11 @@ int rt2x00lib_enable_radio(struct rt2x00_dev *rt2x00dev)
 	 */
 	rt2x00lib_toggle_rx(rt2x00dev, STATE_RADIO_RX_ON);
 
+	/*
+	 * Start watchdog monitoring.
+	 */
+	rt2x00link_start_watchdog(rt2x00dev);
+
 	/*
 	 * Start the TX queues.
 	 */
@@ -88,6 +93,11 @@ void rt2x00lib_disable_radio(struct rt2x00_dev *rt2x00dev)
 	ieee80211_stop_queues(rt2x00dev->hw);
 	rt2x00queue_stop_queues(rt2x00dev);
 
+	/*
+	 * Stop watchdog monitoring.
+	 */
+	rt2x00link_stop_watchdog(rt2x00dev);
+
 	/*
 	 * Disable RX.
 	 */
diff --git a/drivers/net/wireless/rt2x00/rt2x00lib.h b/drivers/net/wireless/rt2x00/rt2x00lib.h
index ed27de1de57..dc5c6574aaf 100644
--- a/drivers/net/wireless/rt2x00/rt2x00lib.h
+++ b/drivers/net/wireless/rt2x00/rt2x00lib.h
@@ -30,6 +30,7 @@
 /*
  * Interval defines
  */
+#define WATCHDOG_INTERVAL	round_jiffies_relative(HZ)
 #define LINK_TUNE_INTERVAL	round_jiffies_relative(HZ)
 
 /*
@@ -257,11 +258,30 @@ void rt2x00link_stop_tuner(struct rt2x00_dev *rt2x00dev);
 void rt2x00link_reset_tuner(struct rt2x00_dev *rt2x00dev, bool antenna);
 
 /**
- * rt2x00link_register - Initialize link tuning functionality
+ * rt2x00link_start_watchdog - Start periodic watchdog monitoring
  * @rt2x00dev: Pointer to &struct rt2x00_dev.
  *
- * Initialize work structure and all link tuning related
- * parameters. This will not start the link tuning process itself.
+ * This start the watchdog periodic work, this work will
+ *be executed periodically until &rt2x00link_stop_watchdog has
+ * been called.
+ */
+void rt2x00link_start_watchdog(struct rt2x00_dev *rt2x00dev);
+
+/**
+ * rt2x00link_stop_watchdog - Stop periodic watchdog monitoring
+ * @rt2x00dev: Pointer to &struct rt2x00_dev.
+ *
+ * After this function completed the watchdog monitoring will not
+ * be running until &rt2x00link_start_watchdog is called.
+ */
+void rt2x00link_stop_watchdog(struct rt2x00_dev *rt2x00dev);
+
+/**
+ * rt2x00link_register - Initialize link tuning & watchdog functionality
+ * @rt2x00dev: Pointer to &struct rt2x00_dev.
+ *
+ * Initialize work structure and all link tuning and watchdog related
+ * parameters. This will not start the periodic work itself.
  */
 void rt2x00link_register(struct rt2x00_dev *rt2x00dev);
 
diff --git a/drivers/net/wireless/rt2x00/rt2x00link.c b/drivers/net/wireless/rt2x00/rt2x00link.c
index 9acfc5c7038..666cef3f847 100644
--- a/drivers/net/wireless/rt2x00/rt2x00link.c
+++ b/drivers/net/wireless/rt2x00/rt2x00link.c
@@ -407,7 +407,45 @@ static void rt2x00link_tuner(struct work_struct *work)
 					     &link->work, LINK_TUNE_INTERVAL);
 }
 
+void rt2x00link_start_watchdog(struct rt2x00_dev *rt2x00dev)
+{
+	struct link *link = &rt2x00dev->link;
+
+	if (!test_bit(DEVICE_STATE_PRESENT, &rt2x00dev->flags) ||
+	    !test_bit(DRIVER_SUPPORT_WATCHDOG, &rt2x00dev->flags))
+		return;
+
+	ieee80211_queue_delayed_work(rt2x00dev->hw,
+				     &link->watchdog_work, WATCHDOG_INTERVAL);
+}
+
+void rt2x00link_stop_watchdog(struct rt2x00_dev *rt2x00dev)
+{
+	cancel_delayed_work_sync(&rt2x00dev->link.watchdog_work);
+}
+
+static void rt2x00link_watchdog(struct work_struct *work)
+{
+	struct rt2x00_dev *rt2x00dev =
+	    container_of(work, struct rt2x00_dev, link.watchdog_work.work);
+	struct link *link = &rt2x00dev->link;
+
+	/*
+	 * When the radio is shutting down we should
+	 * immediately cease the watchdog monitoring.
+	 */
+	if (!test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
+		return;
+
+	rt2x00dev->ops->lib->watchdog(rt2x00dev);
+
+	if (test_bit(DEVICE_STATE_PRESENT, &rt2x00dev->flags))
+		ieee80211_queue_delayed_work(rt2x00dev->hw,
+					     &link->watchdog_work, WATCHDOG_INTERVAL);
+}
+
 void rt2x00link_register(struct rt2x00_dev *rt2x00dev)
 {
+	INIT_DELAYED_WORK(&rt2x00dev->link.watchdog_work, rt2x00link_watchdog);
 	INIT_DELAYED_WORK(&rt2x00dev->link.work, rt2x00link_tuner);
 }
diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.c b/drivers/net/wireless/rt2x00/rt2x00queue.c
index 5097fe0f9f5..a3401d30105 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.c
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.c
@@ -688,9 +688,11 @@ void rt2x00queue_index_inc(struct data_queue *queue, enum queue_index index)
 
 	if (index == Q_INDEX) {
 		queue->length++;
+		queue->last_index = jiffies;
 	} else if (index == Q_INDEX_DONE) {
 		queue->length--;
 		queue->count++;
+		queue->last_index_done = jiffies;
 	}
 
 	spin_unlock_irqrestore(&queue->lock, irqflags);
@@ -704,6 +706,8 @@ static void rt2x00queue_reset(struct data_queue *queue)
 
 	queue->count = 0;
 	queue->length = 0;
+	queue->last_index = jiffies;
+	queue->last_index_done = jiffies;
 	memset(queue->index, 0, sizeof(queue->index));
 
 	spin_unlock_irqrestore(&queue->lock, irqflags);
diff --git a/drivers/net/wireless/rt2x00/rt2x00queue.h b/drivers/net/wireless/rt2x00/rt2x00queue.h
index bd54f55a8cb..191e7775a9c 100644
--- a/drivers/net/wireless/rt2x00/rt2x00queue.h
+++ b/drivers/net/wireless/rt2x00/rt2x00queue.h
@@ -446,6 +446,8 @@ struct data_queue {
 	enum data_queue_qid qid;
 
 	spinlock_t lock;
+	unsigned long last_index;
+	unsigned long last_index_done;
 	unsigned int count;
 	unsigned short limit;
 	unsigned short threshold;
@@ -598,6 +600,15 @@ static inline int rt2x00queue_threshold(struct data_queue *queue)
 	return rt2x00queue_available(queue) < queue->threshold;
 }
 
+/**
+ * rt2x00queue_timeout - Check if a timeout occured for this queue
+ * @queue: Queue to check.
+ */
+static inline int rt2x00queue_timeout(struct data_queue *queue)
+{
+	return time_after(queue->last_index, queue->last_index_done + (HZ / 10));
+}
+
 /**
  * _rt2x00_desc_read - Read a word from the hardware descriptor.
  * @desc: Base descriptor address
diff --git a/drivers/net/wireless/rt2x00/rt2x00usb.c b/drivers/net/wireless/rt2x00/rt2x00usb.c
index a22837c560f..ff3a36622d1 100644
--- a/drivers/net/wireless/rt2x00/rt2x00usb.c
+++ b/drivers/net/wireless/rt2x00/rt2x00usb.c
@@ -292,6 +292,56 @@ void rt2x00usb_kill_tx_queue(struct rt2x00_dev *rt2x00dev,
 }
 EXPORT_SYMBOL_GPL(rt2x00usb_kill_tx_queue);
 
+static void rt2x00usb_watchdog_reset_tx(struct data_queue *queue)
+{
+	struct queue_entry_priv_usb *entry_priv;
+	unsigned short threshold = queue->threshold;
+
+	WARNING(queue->rt2x00dev, "TX queue %d timed out, invoke reset", queue->qid);
+
+	/*
+	 * Temporarily disable the TX queue, this will force mac80211
+	 * to use the other queues until this queue has been restored.
+	 *
+	 * Set the queue threshold to the queue limit. This prevents the
+	 * queue from being enabled during the txdone handler.
+	 */
+	queue->threshold = queue->limit;
+	ieee80211_stop_queue(queue->rt2x00dev->hw, queue->qid);
+
+	/*
+	 * Reset all currently uploaded TX frames.
+	 */
+	while (!rt2x00queue_empty(queue)) {
+		entry_priv = rt2x00queue_get_entry(queue, Q_INDEX_DONE)->priv_data;
+		usb_kill_urb(entry_priv->urb);
+
+		/*
+		 * We need a short delay here to wait for
+		 * the URB to be canceled and invoked the tx_done handler.
+		 */
+		udelay(200);
+	}
+
+	/*
+	 * The queue has been reset, and mac80211 is allowed to use the
+	 * queue again.
+	 */
+	queue->threshold = threshold;
+	ieee80211_wake_queue(queue->rt2x00dev->hw, queue->qid);
+}
+
+void rt2x00usb_watchdog(struct rt2x00_dev *rt2x00dev)
+{
+	struct data_queue *queue;
+
+	tx_queue_for_each(rt2x00dev, queue) {
+		if (rt2x00queue_timeout(queue))
+			rt2x00usb_watchdog_reset_tx(queue);
+	}
+}
+EXPORT_SYMBOL_GPL(rt2x00usb_watchdog);
+
 /*
  * RX data handlers.
  */
diff --git a/drivers/net/wireless/rt2x00/rt2x00usb.h b/drivers/net/wireless/rt2x00/rt2x00usb.h
index 2b7a1889e72..d3d3ddc4087 100644
--- a/drivers/net/wireless/rt2x00/rt2x00usb.h
+++ b/drivers/net/wireless/rt2x00/rt2x00usb.h
@@ -399,6 +399,16 @@ void rt2x00usb_kick_tx_queue(struct rt2x00_dev *rt2x00dev,
 void rt2x00usb_kill_tx_queue(struct rt2x00_dev *rt2x00dev,
 			      const enum data_queue_qid qid);
 
+/**
+ * rt2x00usb_watchdog - Watchdog for USB communication
+ * @rt2x00dev: Pointer to &struct rt2x00_dev
+ *
+ * Check the health of the USB communication and determine
+ * if timeouts have occured. If this is the case, this function
+ * will reset all communication to restore functionality again.
+ */
+void rt2x00usb_watchdog(struct rt2x00_dev *rt2x00dev);
+
 /*
  * Device initialization handlers.
  */
diff --git a/drivers/net/wireless/rt2x00/rt73usb.c b/drivers/net/wireless/rt2x00/rt73usb.c
index 9ea6a672d4e..cad07b69c53 100644
--- a/drivers/net/wireless/rt2x00/rt73usb.c
+++ b/drivers/net/wireless/rt2x00/rt73usb.c
@@ -2136,6 +2136,7 @@ static int rt73usb_probe_hw(struct rt2x00_dev *rt2x00dev)
 	if (!modparam_nohwcrypt)
 		__set_bit(CONFIG_SUPPORT_HW_CRYPTO, &rt2x00dev->flags);
 	__set_bit(DRIVER_SUPPORT_LINK_TUNING, &rt2x00dev->flags);
+	__set_bit(DRIVER_SUPPORT_WATCHDOG, &rt2x00dev->flags);
 
 	/*
 	 * Set the rssi offset.
@@ -2251,6 +2252,7 @@ static const struct rt2x00lib_ops rt73usb_rt2x00_ops = {
 	.link_stats		= rt73usb_link_stats,
 	.reset_tuner		= rt73usb_reset_tuner,
 	.link_tuner		= rt73usb_link_tuner,
+	.watchdog		= rt2x00usb_watchdog,
 	.write_tx_desc		= rt73usb_write_tx_desc,
 	.write_beacon		= rt73usb_write_beacon,
 	.get_tx_data_len	= rt73usb_get_tx_data_len,
-- 
cgit v1.2.3-70-g09d2