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 8dce0077b023..b142a78ed1e5 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);