@@ -1141,9 +1141,9 @@ struct ath5k_hw {
int (*ah_setup_rx_desc)(struct ath5k_hw *ah, struct ath5k_desc *desc,
u32 size, unsigned int flags);
int (*ah_setup_tx_desc)(struct ath5k_hw *, struct ath5k_desc *,
- unsigned int, unsigned int, enum ath5k_pkt_type, unsigned int,
+ unsigned int, unsigned int, int, enum ath5k_pkt_type,
unsigned int, unsigned int, unsigned int, unsigned int,
- unsigned int, unsigned int, unsigned int);
+ unsigned int, unsigned int, unsigned int, unsigned int);
int (*ah_setup_mrr_tx_desc)(struct ath5k_hw *, struct ath5k_desc *,
unsigned int, unsigned int, unsigned int, unsigned int,
unsigned int, unsigned int);
@@ -1370,9 +1370,4 @@ static inline u32 ath5k_hw_bitswap(u32 val, unsigned int bits)
return retval;
}
-static inline int ath5k_pad_size(int hdrlen)
-{
- return (hdrlen < 24) ? 0 : hdrlen & 3;
-}
-
#endif
@@ -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)
{
@@ -1272,7 +1272,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;
@@ -1324,7 +1324,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,
@@ -1806,6 +1806,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)
{
@@ -1819,8 +1880,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);
@@ -1905,12 +1964,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);
/*
@@ -2030,6 +2085,12 @@ ath5k_tx_processq(struct ath5k_softc *sc, struct ath5k_txq *txq)
info->status.ack_signal = ts.ts_rssi;
}
+ /*
+ * Remove MAC header padding before giving the frame
+ * back to mac80211.
+ */
+ ath5k_remove_padding(skb);
+
ieee80211_tx_status(sc->hw, skb);
spin_lock(&sc->txbuflock);
@@ -2073,6 +2134,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);
@@ -2120,7 +2182,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,
@@ -2680,7 +2742,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);
@@ -2692,17 +2753,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);
@@ -2721,7 +2776,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);
@@ -35,7 +35,8 @@
*/
static int
ath5k_hw_setup_2word_tx_desc(struct ath5k_hw *ah, struct ath5k_desc *desc,
- unsigned int pkt_len, unsigned int hdr_len, enum ath5k_pkt_type type,
+ unsigned int pkt_len, unsigned int hdr_len, int padsize,
+ enum ath5k_pkt_type type,
unsigned int tx_power, unsigned int tx_rate0, unsigned int tx_tries0,
unsigned int key_index, unsigned int antenna_mode, unsigned int flags,
unsigned int rtscts_rate, unsigned int rtscts_duration)
@@ -71,7 +72,7 @@ ath5k_hw_setup_2word_tx_desc(struct ath5k_hw *ah, struct ath5k_desc *desc,
/* Verify and set frame length */
/* remove padding we might have added before */
- frame_len = pkt_len - ath5k_pad_size(hdr_len) + FCS_LEN;
+ frame_len = pkt_len - padsize + FCS_LEN;
if (frame_len & ~AR5K_2W_TX_DESC_CTL0_FRAME_LEN)
return -EINVAL;
@@ -100,7 +101,7 @@ ath5k_hw_setup_2word_tx_desc(struct ath5k_hw *ah, struct ath5k_desc *desc,
AR5K_REG_SM(hdr_len, AR5K_2W_TX_DESC_CTL0_HEADER_LEN);
}
- /*Diferences between 5210-5211*/
+ /*Differences between 5210-5211*/
if (ah->ah_version == AR5K_AR5210) {
switch (type) {
case AR5K_PKT_TYPE_BEACON:
@@ -165,6 +166,7 @@ ath5k_hw_setup_2word_tx_desc(struct ath5k_hw *ah, struct ath5k_desc *desc,
*/
static int ath5k_hw_setup_4word_tx_desc(struct ath5k_hw *ah,
struct ath5k_desc *desc, unsigned int pkt_len, unsigned int hdr_len,
+ int padsize,
enum ath5k_pkt_type type, unsigned int tx_power, unsigned int tx_rate0,
unsigned int tx_tries0, unsigned int key_index,
unsigned int antenna_mode, unsigned int flags,
@@ -206,7 +208,7 @@ static int ath5k_hw_setup_4word_tx_desc(struct ath5k_hw *ah,
/* Verify and set frame length */
/* remove padding we might have added before */
- frame_len = pkt_len - ath5k_pad_size(hdr_len) + FCS_LEN;
+ frame_len = pkt_len - padsize + FCS_LEN;
if (frame_len & ~AR5K_4W_TX_DESC_CTL0_FRAME_LEN)
return -EINVAL;