1 files changed, 118 insertions, 32 deletions

diff --git a/drivers/net/wireless/ath/ath5k/base.c b/drivers/net/wireless/ath/ath5k/base.c
index 8dce0077b02..b142a78ed1e 100644
--- a/drivers/net/wireless/ath/ath5k/base.c
+++ b/drivers/net/wireless/ath/ath5k/base.c
@@ -198,7 +198,7 @@ static void __devexit	ath5k_pci_remove(struct pci_dev *pdev);
 static int		ath5k_pci_suspend(struct device *dev);
 static int		ath5k_pci_resume(struct device *dev);
 
-SIMPLE_DEV_PM_OPS(ath5k_pm_ops, ath5k_pci_suspend, ath5k_pci_resume);
+static SIMPLE_DEV_PM_OPS(ath5k_pm_ops, ath5k_pci_suspend, ath5k_pci_resume);
 #define ATH5K_PM_OPS	(&ath5k_pm_ops)
 #else
 #define ATH5K_PM_OPS	NULL
@@ -307,7 +307,7 @@ static int 	ath5k_rxbuf_setup(struct ath5k_softc *sc,
 				struct ath5k_buf *bf);
 static int 	ath5k_txbuf_setup(struct ath5k_softc *sc,
 				struct ath5k_buf *bf,
-				struct ath5k_txq *txq);
+				struct ath5k_txq *txq, int padsize);
 static inline void ath5k_txbuf_free(struct ath5k_softc *sc,
 				struct ath5k_buf *bf)
 {
@@ -1137,8 +1137,6 @@ ath5k_mode_setup(struct ath5k_softc *sc)
 	struct ath5k_hw *ah = sc->ah;
 	u32 rfilt;
 
-	ah->ah_op_mode = sc->opmode;
-
 	/* configure rx filter */
 	rfilt = sc->filter_flags;
 	ath5k_hw_set_rx_filter(ah, rfilt);
@@ -1147,8 +1145,9 @@ ath5k_mode_setup(struct ath5k_softc *sc)
 		ath5k_hw_set_bssid_mask(ah, sc->bssidmask);
 
 	/* configure operational mode */
-	ath5k_hw_set_opmode(ah);
+	ath5k_hw_set_opmode(ah, sc->opmode);
 
+	ATH5K_DBG(sc, ATH5K_DEBUG_MODE, "mode setup opmode %d\n", sc->opmode);
 	ATH5K_DBG(sc, ATH5K_DEBUG_MODE, "RX filter 0x%x\n", rfilt);
 }
 
@@ -1271,7 +1270,7 @@ static enum ath5k_pkt_type get_hw_packet_type(struct sk_buff *skb)
 
 static int
 ath5k_txbuf_setup(struct ath5k_softc *sc, struct ath5k_buf *bf,
-		  struct ath5k_txq *txq)
+		  struct ath5k_txq *txq, int padsize)
 {
 	struct ath5k_hw *ah = sc->ah;
 	struct ath5k_desc *ds = bf->desc;
@@ -1323,7 +1322,7 @@ ath5k_txbuf_setup(struct ath5k_softc *sc, struct ath5k_buf *bf,
 			sc->vif, pktlen, info));
 	}
 	ret = ah->ah_setup_tx_desc(ah, ds, pktlen,
-		ieee80211_get_hdrlen_from_skb(skb),
+		ieee80211_get_hdrlen_from_skb(skb), padsize,
 		get_hw_packet_type(skb),
 		(sc->power_level * 2),
 		hw_rate,
@@ -1805,6 +1804,67 @@ ath5k_check_ibss_tsf(struct ath5k_softc *sc, struct sk_buff *skb,
 	}
 }
 
+/*
+ * Compute padding position. skb must contains an IEEE 802.11 frame
+ */
+static int ath5k_common_padpos(struct sk_buff *skb)
+{
+	struct ieee80211_hdr * hdr = (struct ieee80211_hdr *)skb->data;
+	__le16 frame_control = hdr->frame_control;
+	int padpos = 24;
+
+	if (ieee80211_has_a4(frame_control)) {
+		padpos += ETH_ALEN;
+	}
+	if (ieee80211_is_data_qos(frame_control)) {
+		padpos += IEEE80211_QOS_CTL_LEN;
+	}
+
+	return padpos;
+}
+
+/*
+ * This function expects a 802.11 frame and returns the number of
+ * bytes added, or -1 if we don't have enought header room.
+ */
+
+static int ath5k_add_padding(struct sk_buff *skb)
+{
+	int padpos = ath5k_common_padpos(skb);
+	int padsize = padpos & 3;
+
+	if (padsize && skb->len>padpos) {
+
+		if (skb_headroom(skb) < padsize)
+			return -1;
+
+		skb_push(skb, padsize);
+		memmove(skb->data, skb->data+padsize, padpos);
+		return padsize;
+	}
+
+	return 0;
+}
+
+/*
+ * This function expects a 802.11 frame and returns the number of
+ * bytes removed
+ */
+
+static int ath5k_remove_padding(struct sk_buff *skb)
+{
+	int padpos = ath5k_common_padpos(skb);
+	int padsize = padpos & 3;
+
+	if (padsize && skb->len>=padpos+padsize) {
+		memmove(skb->data + padsize, skb->data, padpos);
+		skb_pull(skb, padsize);
+		return padsize;
+	}
+
+	return 0;
+}
+
 static void
 ath5k_tasklet_rx(unsigned long data)
 {
@@ -1818,8 +1878,6 @@ ath5k_tasklet_rx(unsigned long data)
 	struct ath5k_buf *bf;
 	struct ath5k_desc *ds;
 	int ret;
-	int hdrlen;
-	int padsize;
 	int rx_flag;
 
 	spin_lock(&sc->rxbuflock);
@@ -1844,18 +1902,28 @@ ath5k_tasklet_rx(unsigned long data)
 			break;
 		else if (unlikely(ret)) {
 			ATH5K_ERR(sc, "error in processing rx descriptor\n");
+			sc->stats.rxerr_proc++;
 			spin_unlock(&sc->rxbuflock);
 			return;
 		}
 
+		sc->stats.rx_all_count++;
+
 		if (unlikely(rs.rs_more)) {
 			ATH5K_WARN(sc, "unsupported jumbo\n");
+			sc->stats.rxerr_jumbo++;
 			goto next;
 		}
 
 		if (unlikely(rs.rs_status)) {
-			if (rs.rs_status & AR5K_RXERR_PHY)
+			if (rs.rs_status & AR5K_RXERR_CRC)
+				sc->stats.rxerr_crc++;
+			if (rs.rs_status & AR5K_RXERR_FIFO)
+				sc->stats.rxerr_fifo++;
+			if (rs.rs_status & AR5K_RXERR_PHY) {
+				sc->stats.rxerr_phy++;
 				goto next;
+			}
 			if (rs.rs_status & AR5K_RXERR_DECRYPT) {
 				/*
 				 * Decrypt error.  If the error occurred
@@ -1867,12 +1935,14 @@ ath5k_tasklet_rx(unsigned long data)
 				 *
 				 * XXX do key cache faulting
 				 */
+				sc->stats.rxerr_decrypt++;
 				if (rs.rs_keyix == AR5K_RXKEYIX_INVALID &&
 				    !(rs.rs_status & AR5K_RXERR_CRC))
 					goto accept;
 			}
 			if (rs.rs_status & AR5K_RXERR_MIC) {
 				rx_flag |= RX_FLAG_MMIC_ERROR;
+				sc->stats.rxerr_mic++;
 				goto accept;
 			}
 
@@ -1904,12 +1974,8 @@ accept:
 		 * bytes and we can optimize this a bit. In addition, we must
 		 * not try to remove padding from short control frames that do
 		 * not have payload. */
-		hdrlen = ieee80211_get_hdrlen_from_skb(skb);
-		padsize = ath5k_pad_size(hdrlen);
-		if (padsize) {
-			memmove(skb->data + padsize, skb->data, hdrlen);
-			skb_pull(skb, padsize);
-		}
+		ath5k_remove_padding(skb);
+
 		rxs = IEEE80211_SKB_RXCB(skb);
 
 		/*
@@ -1942,6 +2008,12 @@ accept:
 		rxs->signal = rxs->noise + rs.rs_rssi;
 
 		rxs->antenna = rs.rs_antenna;
+
+		if (rs.rs_antenna > 0 && rs.rs_antenna < 5)
+			sc->stats.antenna_rx[rs.rs_antenna]++;
+		else
+			sc->stats.antenna_rx[0]++; /* invalid */
+
 		rxs->rate_idx = ath5k_hw_to_driver_rix(sc, rs.rs_rate);
 		rxs->flag |= ath5k_rx_decrypted(sc, ds, skb, &rs);
 
@@ -1996,6 +2068,7 @@ ath5k_tx_processq(struct ath5k_softc *sc, struct ath5k_txq *txq)
 			break;
 		}
 
+		sc->stats.tx_all_count++;
 		skb = bf->skb;
 		info = IEEE80211_SKB_CB(skb);
 		bf->skb = NULL;
@@ -2022,13 +2095,30 @@ ath5k_tx_processq(struct ath5k_softc *sc, struct ath5k_txq *txq)
 
 		if (unlikely(ts.ts_status)) {
 			sc->ll_stats.dot11ACKFailureCount++;
-			if (ts.ts_status & AR5K_TXERR_FILT)
+			if (ts.ts_status & AR5K_TXERR_FILT) {
 				info->flags |= IEEE80211_TX_STAT_TX_FILTERED;
+				sc->stats.txerr_filt++;
+			}
+			if (ts.ts_status & AR5K_TXERR_XRETRY)
+				sc->stats.txerr_retry++;
+			if (ts.ts_status & AR5K_TXERR_FIFO)
+				sc->stats.txerr_fifo++;
 		} else {
 			info->flags |= IEEE80211_TX_STAT_ACK;
 			info->status.ack_signal = ts.ts_rssi;
 		}
 
+		/*
+		 * Remove MAC header padding before giving the frame
+		 * back to mac80211.
+		 */
+		ath5k_remove_padding(skb);
+
+		if (ts.ts_antenna > 0 && ts.ts_antenna < 5)
+			sc->stats.antenna_tx[ts.ts_antenna]++;
+		else
+			sc->stats.antenna_tx[0]++; /* invalid */
+
 		ieee80211_tx_status(sc->hw, skb);
 
 		spin_lock(&sc->txbuflock);
@@ -2072,6 +2162,7 @@ ath5k_beacon_setup(struct ath5k_softc *sc, struct ath5k_buf *bf)
 	int ret = 0;
 	u8 antenna;
 	u32 flags;
+	const int padsize = 0;
 
 	bf->skbaddr = pci_map_single(sc->pdev, skb->data, skb->len,
 			PCI_DMA_TODEVICE);
@@ -2119,7 +2210,7 @@ ath5k_beacon_setup(struct ath5k_softc *sc, struct ath5k_buf *bf)
 	 * from tx power (value is in dB units already) */
 	ds->ds_data = bf->skbaddr;
 	ret = ah->ah_setup_tx_desc(ah, ds, skb->len,
-			ieee80211_get_hdrlen_from_skb(skb),
+			ieee80211_get_hdrlen_from_skb(skb), padsize,
 			AR5K_PKT_TYPE_BEACON, (sc->power_level * 2),
 			ieee80211_get_tx_rate(sc->hw, info)->hw_value,
 			1, AR5K_TXKEYIX_INVALID,
@@ -2679,7 +2770,6 @@ static int ath5k_tx_queue(struct ieee80211_hw *hw, struct sk_buff *skb,
 	struct ath5k_softc *sc = hw->priv;
 	struct ath5k_buf *bf;
 	unsigned long flags;
-	int hdrlen;
 	int padsize;
 
 	ath5k_debug_dump_skb(sc, skb, "TX  ", 1);
@@ -2691,17 +2781,11 @@ static int ath5k_tx_queue(struct ieee80211_hw *hw, struct sk_buff *skb,
 	 * the hardware expects the header padded to 4 byte boundaries
 	 * if this is not the case we add the padding after the header
 	 */
-	hdrlen = ieee80211_get_hdrlen_from_skb(skb);
-	padsize = ath5k_pad_size(hdrlen);
-	if (padsize) {
-
-		if (skb_headroom(skb) < padsize) {
-			ATH5K_ERR(sc, "tx hdrlen not %%4: %d not enough"
-				  " headroom to pad %d\n", hdrlen, padsize);
-			goto drop_packet;
-		}
-		skb_push(skb, padsize);
-		memmove(skb->data, skb->data+padsize, hdrlen);
+	padsize = ath5k_add_padding(skb);
+	if (padsize < 0) {
+		ATH5K_ERR(sc, "tx hdrlen not %%4: not enough"
+			  " headroom to pad");
+		goto drop_packet;
 	}
 
 	spin_lock_irqsave(&sc->txbuflock, flags);
@@ -2720,7 +2804,7 @@ static int ath5k_tx_queue(struct ieee80211_hw *hw, struct sk_buff *skb,
 
 	bf->skb = skb;
 
-	if (ath5k_txbuf_setup(sc, bf, txq)) {
+	if (ath5k_txbuf_setup(sc, bf, txq, padsize)) {
 		bf->skb = NULL;
 		spin_lock_irqsave(&sc->txbuflock, flags);
 		list_add_tail(&bf->list, &sc->txbuf);
@@ -2835,6 +2919,8 @@ static int ath5k_add_interface(struct ieee80211_hw *hw,
 		goto end;
 	}
 
+	ATH5K_DBG(sc, ATH5K_DEBUG_MODE, "add interface mode %d\n", sc->opmode);
+
 	ath5k_hw_set_lladdr(sc->ah, vif->addr);
 	ath5k_mode_setup(sc);
 
@@ -2905,7 +2991,7 @@ ath5k_config(struct ieee80211_hw *hw, u32 changed)
 	 * then we must allow the user to set how many tx antennas we
 	 * have available
 	 */
-	ath5k_hw_set_antenna_mode(ah, AR5K_ANTMODE_DEFAULT);
+	ath5k_hw_set_antenna_mode(ah, ah->ah_ant_mode);
 
 unlock:
 	mutex_unlock(&sc->lock);