1 files changed, 33 insertions, 38 deletions

diff --git a/drivers/net/wireless/ipw2200.c b/drivers/net/wireless/ipw2200.c
index a879edba5fac..0500e8006a14 100644
--- a/drivers/net/wireless/ipw2200.c
+++ b/drivers/net/wireless/ipw2200.c
@@ -6859,61 +6859,55 @@ static int ipw_get_tx_queue_number(struct ipw_priv *priv, u16 priority)
         return from_priority_to_tx_queue[priority] - 1;
 }
 
-/*
-* add QoS parameter to the TX command
-*/
-static int ipw_qos_set_tx_queue_command(struct ipw_priv *priv,
-                                        u16 priority,
-                                        struct tfd_data *tfd, u8 unicast)
+static int ipw_is_qos_active(struct net_device *dev,
+                             struct sk_buff *skb)
 {
-        int ret = 0;
-        int tx_queue_id = 0;
+        struct ipw_priv *priv = ieee80211_priv(dev);
         struct ieee80211_qos_data *qos_data = NULL;
         int active, supported;
-        unsigned long flags;
+        u8 *daddr = skb->data + ETH_ALEN;
+        int unicast = !is_multicast_ether_addr(daddr);
 
         if (!(priv->status & STATUS_ASSOCIATED))
                 return 0;
 
         qos_data = &priv->assoc_network->qos_data;
 
-        spin_lock_irqsave(&priv->ieee->lock, flags);
-
         if (priv->ieee->iw_mode == IW_MODE_ADHOC) {
                 if (unicast == 0)
                         qos_data->active = 0;
                 else
                         qos_data->active = qos_data->supported;
         }
-
         active = qos_data->active;
         supported = qos_data->supported;
-
-        spin_unlock_irqrestore(&priv->ieee->lock, flags);
-
         IPW_DEBUG_QOS("QoS  %d network is QoS active %d  supported %d  "
                       "unicast %d\n",
                       priv->qos_data.qos_enable, active, supported, unicast);
-        if (active && priv->qos_data.qos_enable) {
-                ret = from_priority_to_tx_queue[priority];
-                tx_queue_id = ret - 1;
-                IPW_DEBUG_QOS("QoS packet priority is %d \n", priority);
-                if (priority <= 7) {
-                        tfd->tx_flags_ext |= DCT_FLAG_EXT_QOS_ENABLED;
-                        tfd->tfd.tfd_26.mchdr.qos_ctrl = priority;
-                        tfd->tfd.tfd_26.mchdr.frame_ctl |=
-                            IEEE80211_STYPE_QOS_DATA;
-
-                        if (priv->qos_data.qos_no_ack_mask &
-                            (1UL << tx_queue_id)) {
-                                tfd->tx_flags &= ~DCT_FLAG_ACK_REQD;
-                                tfd->tfd.tfd_26.mchdr.qos_ctrl |=
-                                    CTRL_QOS_NO_ACK;
-                        }
-                }
-        }
+        if (active && priv->qos_data.qos_enable)
+                return 1;
 
-        return ret;
+        return 0;
+
+}
+/*
+* add QoS parameter to the TX command
+*/
+static int ipw_qos_set_tx_queue_command(struct ipw_priv *priv,
+                                        u16 priority,
+                                        struct tfd_data *tfd)
+{
+        int tx_queue_id = 0;
+
+
+        tx_queue_id = from_priority_to_tx_queue[priority] - 1;
+        tfd->tx_flags_ext |= DCT_FLAG_EXT_QOS_ENABLED;
+
+        if (priv->qos_data.qos_no_ack_mask & (1UL << tx_queue_id)) {
+                tfd->tx_flags &= ~DCT_FLAG_ACK_REQD;
+                tfd->tfd.tfd_26.mchdr.qos_ctrl |= CTRL_QOS_NO_ACK;
+        }
+        return 0;
 }
 
 /*
@@ -9653,7 +9647,7 @@ we need to heavily modify the ieee80211_skb_to_txb.
 static int ipw_tx_skb(struct ipw_priv *priv, struct ieee80211_txb *txb,
                              int pri)
 {
-        struct ieee80211_hdr_3addr *hdr = (struct ieee80211_hdr_3addr *)
+        struct ieee80211_hdr_3addrqos *hdr = (struct ieee80211_hdr_3addrqos *)
             txb->fragments[0]->data;
         int i = 0;
         struct tfd_frame *tfd;
@@ -9668,9 +9662,9 @@ static int ipw_tx_skb(struct ipw_priv *priv, struct ieee80211_txb *txb,
         u16 remaining_bytes;
         int fc;
 
+        hdr_len = ieee80211_get_hdrlen(le16_to_cpu(hdr->frame_ctl));
         switch (priv->ieee->iw_mode) {
         case IW_MODE_ADHOC:
-                hdr_len = IEEE80211_3ADDR_LEN;
                 unicast = !is_multicast_ether_addr(hdr->addr1);
                 id = ipw_find_station(priv, hdr->addr1);
                 if (id == IPW_INVALID_STATION) {
@@ -9687,7 +9681,6 @@ static int ipw_tx_skb(struct ipw_priv *priv, struct ieee80211_txb *txb,
         case IW_MODE_INFRA:
         default:
                 unicast = !is_multicast_ether_addr(hdr->addr3);
-                hdr_len = IEEE80211_3ADDR_LEN;
                 id = 0;
                 break;
         }
@@ -9766,7 +9759,8 @@ static int ipw_tx_skb(struct ipw_priv *priv, struct ieee80211_txb *txb,
                 tfd->u.data.tx_flags |= DCT_FLAG_NO_WEP;
 
 #ifdef CONFIG_IPW_QOS
-        ipw_qos_set_tx_queue_command(priv, pri, &(tfd->u.data), unicast);
+        if (fc & IEEE80211_STYPE_QOS_DATA)
+                ipw_qos_set_tx_queue_command(priv, pri, &(tfd->u.data));
 #endif                          /* CONFIG_IPW_QOS */
 
         /* payload */
@@ -10966,6 +10960,7 @@ static int ipw_pci_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
         priv->ieee->is_queue_full = ipw_net_is_queue_full;
 
 #ifdef CONFIG_IPW_QOS
+        priv->ieee->is_qos_active = ipw_is_qos_active;
         priv->ieee->handle_probe_response = ipw_handle_beacon;
         priv->ieee->handle_beacon = ipw_handle_probe_response;
         priv->ieee->handle_assoc_response = ipw_handle_assoc_response;