p54: put broadcast frames into the right queues

stlc45xx's specs finally brought some light what all the 4 extra queues for.
now CAB data and managment frames have their own queue.

Signed-off-by: Christian Lamparter <chunkeey@web.de>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

diff --git a/drivers/net/wireless/p54/p54common.c b/drivers/net/wireless/p54/p54common.c
index ae8d1f666314fa1fe3be02d19a696de5a4368882..e93bca8263cd87292a5d1f6ca01aabd813cf5a97 100644 (file)
--- a/drivers/net/wireless/p54/p54common.c
+++ b/drivers/net/wireless/p54/p54common.c
@@ -902,6 +902,55 @@ free:
 }
 EXPORT_SYMBOL_GPL(p54_read_eeprom);
 
+static int p54_tx_fill(struct ieee80211_hw *dev, struct sk_buff *skb,
+               struct ieee80211_tx_info *info, u8 *queue, size_t *extra_len,
+               u16 *flags, u16 *aid)
+{
+       struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)skb->data;
+       struct p54_common *priv = dev->priv;
+       int ret = 0;
+
+       if (unlikely(ieee80211_is_mgmt(hdr->frame_control))) {
+               if (ieee80211_is_beacon(hdr->frame_control)) {
+                       *aid = 0;
+                       *queue = 0;
+                       *extra_len = IEEE80211_MAX_TIM_LEN;
+                       *flags = P54_HDR_FLAG_DATA_OUT_TIMESTAMP;
+                       return 0;
+               } else if (ieee80211_is_probe_resp(hdr->frame_control)) {
+                       *aid = 0;
+                       *queue = 2;
+                       *flags = P54_HDR_FLAG_DATA_OUT_TIMESTAMP |
+                                P54_HDR_FLAG_DATA_OUT_NOCANCEL;
+                       return 0;
+               } else {
+                       *queue = 2;
+                       ret = 0;
+               }
+       } else {
+               *queue += 4;
+               ret = 1;
+       }
+
+       switch (priv->mode) {
+       case NL80211_IFTYPE_STATION:
+               *aid = 1;
+               break;
+       case NL80211_IFTYPE_AP:
+       case NL80211_IFTYPE_ADHOC:
+               if (info->flags & IEEE80211_TX_CTL_SEND_AFTER_DTIM) {
+                       *aid = 0;
+                       *queue = 3;
+                       return 0;
+               }
+               if (info->control.sta)
+                       *aid = info->control.sta->aid;
+               else
+                       *flags = P54_HDR_FLAG_DATA_OUT_NOCANCEL;
+       }
+       return ret;
+}
+
 static int p54_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
 {
        struct ieee80211_tx_info *info = IEEE80211_SKB_CB(skb);
@@ -909,22 +958,26 @@ static int p54_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
        struct p54_common *priv = dev->priv;
        struct p54_hdr *hdr;
        struct p54_tx_data *txhdr;
-       size_t padding, len;
+       size_t padding, len, tim_len;
        int i, j, ridx;
-       u16 hdr_flags = 0;
-       u8 rate;
+       u16 hdr_flags = 0, aid = 0;
+       u8 rate, queue;
        u8 cts_rate = 0x20;
        u8 rc_flags;
        u8 calculated_tries[4];
        u8 nrates = 0, nremaining = 8;
 
-       current_queue = &priv->tx_stats[skb_get_queue_mapping(skb) + 4];
-       if (unlikely(current_queue->len > current_queue->limit))
-               return NETDEV_TX_BUSY;
-       current_queue->len++;
-       current_queue->count++;
-       if (current_queue->len == current_queue->limit)
-               ieee80211_stop_queue(dev, skb_get_queue_mapping(skb));
+       queue = skb_get_queue_mapping(skb);
+
+       if (p54_tx_fill(dev, skb, info, &queue, &tim_len, &hdr_flags, &aid)) {
+               current_queue = &priv->tx_stats[queue];
+               if (unlikely(current_queue->len > current_queue->limit))
+                       return NETDEV_TX_BUSY;
+               current_queue->len++;
+               current_queue->count++;
+               if (current_queue->len == current_queue->limit)
+                       ieee80211_stop_queue(dev, skb_get_queue_mapping(skb));
+       }
 
        padding = (unsigned long)(skb->data - (sizeof(*hdr) + sizeof(*txhdr))) & 3;
        len = skb->len;
@@ -935,7 +988,7 @@ static int p54_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
        if (padding)
                hdr_flags |= P54_HDR_FLAG_DATA_ALIGN;
        hdr->len = cpu_to_le16(len);
-       hdr->type = (info->flags & IEEE80211_TX_CTL_NO_ACK) ? 0 : cpu_to_le16(1);
+       hdr->type = cpu_to_le16(aid);
        hdr->rts_tries = info->control.rates[0].count;
 
        /*
@@ -1013,7 +1066,7 @@ static int p54_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
        txhdr->rts_rate_idx = 0;
        txhdr->key_type = 0;
        txhdr->key_len = 0;
-       txhdr->hw_queue = skb_get_queue_mapping(skb) + 4;
+       txhdr->hw_queue = queue;
        txhdr->backlog = 32;
        memset(txhdr->durations, 0, sizeof(txhdr->durations));
        txhdr->tx_antenna = (info->antenna_sel_tx == 0) ?
@@ -1024,7 +1077,7 @@ static int p54_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
                txhdr->align[0] = padding;
 
        /* modifies skb->cb and with it info, so must be last! */
-       if (unlikely(p54_assign_address(dev, skb, hdr, skb->len))) {
+       if (unlikely(p54_assign_address(dev, skb, hdr, skb->len + tim_len))) {
                skb_pull(skb, sizeof(*hdr) + sizeof(*txhdr) + padding);
                if (current_queue) {
                        current_queue->len--;
@@ -1509,8 +1562,7 @@ struct ieee80211_hw *p54_init_common(size_t priv_data_len)
        priv = dev->priv;
        priv->mode = NL80211_IFTYPE_UNSPECIFIED;
        skb_queue_head_init(&priv->tx_queue);
-       dev->flags = IEEE80211_HW_HOST_BROADCAST_PS_BUFFERING | /* not sure */
-                    IEEE80211_HW_RX_INCLUDES_FCS |
+       dev->flags = IEEE80211_HW_RX_INCLUDES_FCS |
                     IEEE80211_HW_SIGNAL_DBM |
                     IEEE80211_HW_NOISE_DBM;