5 files changed, 300 insertions, 147 deletions

diff --git a/drivers/net/wireless/p54/p54.h b/drivers/net/wireless/p54/p54.h
index 10df07de9e52..fca8762fa069 100644
--- a/drivers/net/wireless/p54/p54.h
+++ b/drivers/net/wireless/p54/p54.h
@@ -66,7 +66,7 @@ struct p54_common {
         unsigned int tx_hdr_len;
         void *cached_vdcf;
         unsigned int fw_var;
-        struct ieee80211_tx_queue_stats tx_stats[4];
+        struct ieee80211_tx_queue_stats tx_stats[8];
 };
 
 int p54_rx(struct ieee80211_hw *dev, struct sk_buff *skb);
diff --git a/drivers/net/wireless/p54/p54common.c b/drivers/net/wireless/p54/p54common.c
index 29be3dc8ee09..17e06bbc996a 100644
--- a/drivers/net/wireless/p54/p54common.c
+++ b/drivers/net/wireless/p54/p54common.c
@@ -146,23 +146,23 @@ void p54_parse_firmware(struct ieee80211_hw *dev, const struct firmware *fw)
 
         if (priv->fw_var >= 0x300) {
                 /* Firmware supports QoS, use it! */
-                priv->tx_stats[0].limit = 3;
-                priv->tx_stats[1].limit = 4;
-                priv->tx_stats[2].limit = 3;
-                priv->tx_stats[3].limit = 1;
+                priv->tx_stats[4].limit = 3;
+                priv->tx_stats[5].limit = 4;
+                priv->tx_stats[6].limit = 3;
+                priv->tx_stats[7].limit = 1;
                 dev->queues = 4;
         }
 }
 EXPORT_SYMBOL_GPL(p54_parse_firmware);
 
-static int p54_convert_rev0_to_rev1(struct ieee80211_hw *dev,
-                                    struct pda_pa_curve_data *curve_data)
+static int p54_convert_rev0(struct ieee80211_hw *dev,
+                            struct pda_pa_curve_data *curve_data)
 {
         struct p54_common *priv = dev->priv;
-        struct pda_pa_curve_data_sample_rev1 *rev1;
-        struct pda_pa_curve_data_sample_rev0 *rev0;
+        struct p54_pa_curve_data_sample *dst;
+        struct pda_pa_curve_data_sample_rev0 *src;
         size_t cd_len = sizeof(*curve_data) +
-                (curve_data->points_per_channel*sizeof(*rev1) + 2) *
+                (curve_data->points_per_channel*sizeof(*dst) + 2) *
                  curve_data->channels;
         unsigned int i, j;
         void *source, *target;
@@ -180,27 +180,63 @@ static int p54_convert_rev0_to_rev1(struct ieee80211_hw *dev,
                 *((__le16 *)target) = *freq;
                 target += sizeof(__le16);
                 for (j = 0; j < curve_data->points_per_channel; j++) {
-                        rev1 = target;
-                        rev0 = source;
+                        dst = target;
+                        src = source;
 
-                        rev1->rf_power = rev0->rf_power;
-                        rev1->pa_detector = rev0->pa_detector;
-                        rev1->data_64qam = rev0->pcv;
+                        dst->rf_power = src->rf_power;
+                        dst->pa_detector = src->pa_detector;
+                        dst->data_64qam = src->pcv;
                         /* "invent" the points for the other modulations */
 #define SUB(x,y) (u8)((x) - (y)) > (x) ? 0 : (x) - (y)
-                        rev1->data_16qam = SUB(rev0->pcv, 12);
-                        rev1->data_qpsk  = SUB(rev1->data_16qam, 12);
-                        rev1->data_bpsk  = SUB(rev1->data_qpsk, 12);
-                        rev1->data_barker= SUB(rev1->data_bpsk, 14);
+                        dst->data_16qam = SUB(src->pcv, 12);
+                        dst->data_qpsk = SUB(dst->data_16qam, 12);
+                        dst->data_bpsk = SUB(dst->data_qpsk, 12);
+                        dst->data_barker = SUB(dst->data_bpsk, 14);
 #undef SUB
-                        target += sizeof(*rev1);
-                        source += sizeof(*rev0);
+                        target += sizeof(*dst);
+                        source += sizeof(*src);
                 }
         }
 
         return 0;
 }
 
+static int p54_convert_rev1(struct ieee80211_hw *dev,
+                            struct pda_pa_curve_data *curve_data)
+{
+        struct p54_common *priv = dev->priv;
+        struct p54_pa_curve_data_sample *dst;
+        struct pda_pa_curve_data_sample_rev1 *src;
+        size_t cd_len = sizeof(*curve_data) +
+                (curve_data->points_per_channel*sizeof(*dst) + 2) *
+                 curve_data->channels;
+        unsigned int i, j;
+        void *source, *target;
+
+        priv->curve_data = kmalloc(cd_len, GFP_KERNEL);
+        if (!priv->curve_data)
+                return -ENOMEM;
+
+        memcpy(priv->curve_data, curve_data, sizeof(*curve_data));
+        source = curve_data->data;
+        target = priv->curve_data->data;
+        for (i = 0; i < curve_data->channels; i++) {
+                __le16 *freq = source;
+                source += sizeof(__le16);
+                *((__le16 *)target) = *freq;
+                target += sizeof(__le16);
+                for (j = 0; j < curve_data->points_per_channel; j++) {
+                        memcpy(target, source, sizeof(*src));
+
+                        target += sizeof(*dst);
+                        source += sizeof(*src);
+                }
+                source++;
+        }
+
+        return 0;
+}
+
 int p54_parse_eeprom(struct ieee80211_hw *dev, void *eeprom, int len)
 {
         struct p54_common *priv = dev->priv;
@@ -250,27 +286,32 @@ int p54_parse_eeprom(struct ieee80211_hw *dev, void *eeprom, int len)
                                entry->data[1]*sizeof(*priv->output_limit));
                         priv->output_limit_len = entry->data[1];
                         break;
-                case PDR_PRISM_PA_CAL_CURVE_DATA:
-                        if (data_len < sizeof(struct pda_pa_curve_data)) {
+                case PDR_PRISM_PA_CAL_CURVE_DATA: {
+                        struct pda_pa_curve_data *curve_data =
+                                (struct pda_pa_curve_data *)entry->data;
+                        if (data_len < sizeof(*curve_data)) {
                                 err = -EINVAL;
                                 goto err;
                         }
 
-                        if (((struct pda_pa_curve_data *)entry->data)->cal_method_rev) {
-                                priv->curve_data = kmalloc(data_len, GFP_KERNEL);
-                                if (!priv->curve_data) {
-                                        err = -ENOMEM;
-                                        goto err;
-                                }
-
-                                memcpy(priv->curve_data, entry->data, data_len);
-                        } else {
-                                err = p54_convert_rev0_to_rev1(dev, (struct pda_pa_curve_data *)entry->data);
-                                if (err)
-                                        goto err;
+                        switch (curve_data->cal_method_rev) {
+                        case 0:
+                                err = p54_convert_rev0(dev, curve_data);
+                                break;
+                        case 1:
+                                err = p54_convert_rev1(dev, curve_data);
+                                break;
+                        default:
+                                printk(KERN_ERR "p54: unknown curve data "
+                                                "revision %d\n",
+                                                curve_data->cal_method_rev);
+                                err = -ENODEV;
+                                break;
                         }
+                        if (err)
+                                goto err;
 
-                        break;
+                }
                 case PDR_PRISM_ZIF_TX_IQ_CALIBRATION:
                         priv->iq_autocal = kmalloc(data_len, GFP_KERNEL);
                         if (!priv->iq_autocal) {
@@ -377,7 +418,7 @@ static void inline p54_wake_free_queues(struct ieee80211_hw *dev)
         int i;
 
         for (i = 0; i < dev->queues; i++)
-                if (priv->tx_stats[i].len < priv->tx_stats[i].limit)
+                if (priv->tx_stats[i + 4].len < priv->tx_stats[i + 4].limit)
                         ieee80211_wake_queue(dev, i);
 }
 
@@ -391,7 +432,9 @@ static void p54_rx_frame_sent(struct ieee80211_hw *dev, struct sk_buff *skb)
         struct memrecord *range = NULL;
         u32 freed = 0;
         u32 last_addr = priv->rx_start;
+        unsigned long flags;
 
+        spin_lock_irqsave(&priv->tx_queue.lock, flags);
         while (entry != (struct sk_buff *)&priv->tx_queue) {
                 struct ieee80211_tx_info *info = IEEE80211_SKB_CB(entry);
                 range = (void *)info->driver_data;
@@ -412,13 +455,15 @@ static void p54_rx_frame_sent(struct ieee80211_hw *dev, struct sk_buff *skb)
 
                         last_addr = range->end_addr;
                         __skb_unlink(entry, &priv->tx_queue);
+                        spin_unlock_irqrestore(&priv->tx_queue.lock, flags);
+
                         memset(&info->status, 0, sizeof(info->status));
                         entry_hdr = (struct p54_control_hdr *) entry->data;
                         entry_data = (struct p54_tx_control_allocdata *) entry_hdr->data;
                         if ((entry_hdr->magic1 & cpu_to_le16(0x4000)) != 0)
                                 pad = entry_data->align[0];
 
-                        priv->tx_stats[entry_data->hw_queue - 4].len--;
+                        priv->tx_stats[entry_data->hw_queue].len--;
                         if (!(info->flags & IEEE80211_TX_CTL_NO_ACK)) {
                                 if (!(payload->status & 0x01))
                                         info->flags |= IEEE80211_TX_STAT_ACK;
@@ -429,12 +474,14 @@ static void p54_rx_frame_sent(struct ieee80211_hw *dev, struct sk_buff *skb)
                         info->status.ack_signal = le16_to_cpu(payload->ack_rssi);
                         skb_pull(entry, sizeof(*hdr) + pad + sizeof(*entry_data));
                         ieee80211_tx_status_irqsafe(dev, entry);
-                        break;
+                        goto out;
                 } else
                         last_addr = range->end_addr;
                 entry = entry->next;
         }
+        spin_unlock_irqrestore(&priv->tx_queue.lock, flags);
 
+out:
         if (freed >= IEEE80211_MAX_RTS_THRESHOLD + 0x170 +
             sizeof(struct p54_control_hdr))
                 p54_wake_free_queues(dev);
@@ -559,7 +606,7 @@ static int p54_tx(struct ieee80211_hw *dev, struct sk_buff *skb)
         u8 rate;
         u8 cts_rate = 0x20;
 
-        current_queue = &priv->tx_stats[skb_get_queue_mapping(skb)];
+        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++;
@@ -672,12 +719,9 @@ static int p54_set_freq(struct ieee80211_hw *dev, __le16 freq)
         struct p54_control_hdr *hdr;
         struct p54_tx_control_channel *chan;
         unsigned int i;
-        size_t payload_len = sizeof(*chan) + sizeof(u32)*2 +
-                             sizeof(*chan->curve_data) *
-                             priv->curve_data->points_per_channel;
         void *entry;
 
-        hdr = kzalloc(sizeof(*hdr) + payload_len +
+        hdr = kzalloc(sizeof(*hdr) + sizeof(*chan) +
                       priv->tx_hdr_len, GFP_KERNEL);
         if (!hdr)
                 return -ENOMEM;
@@ -689,10 +733,10 @@ static int p54_set_freq(struct ieee80211_hw *dev, __le16 freq)
         hdr->magic1 = cpu_to_le16(0x8001);
         hdr->len = cpu_to_le16(sizeof(*chan));
         hdr->type = cpu_to_le16(P54_CONTROL_TYPE_CHANNEL_CHANGE);
-        p54_assign_address(dev, NULL, hdr, sizeof(*hdr) + payload_len);
+        p54_assign_address(dev, NULL, hdr, sizeof(*hdr) + sizeof(*chan));
 
-        chan->magic1 = cpu_to_le16(0x1);
-        chan->magic2 = cpu_to_le16(0x0);
+        chan->flags = cpu_to_le16(0x1);
+        chan->dwell = cpu_to_le16(0x0);
 
         for (i = 0; i < priv->iq_autocal_len; i++) {
                 if (priv->iq_autocal[i].freq != freq)
@@ -710,35 +754,41 @@ static int p54_set_freq(struct ieee80211_hw *dev, __le16 freq)
                         continue;
 
                 chan->val_barker = 0x38;
-                chan->val_bpsk = priv->output_limit[i].val_bpsk;
-                chan->val_qpsk = priv->output_limit[i].val_qpsk;
-                chan->val_16qam = priv->output_limit[i].val_16qam;
-                chan->val_64qam = priv->output_limit[i].val_64qam;
+                chan->val_bpsk = chan->dup_bpsk =
+                        priv->output_limit[i].val_bpsk;
+                chan->val_qpsk = chan->dup_qpsk =
+                        priv->output_limit[i].val_qpsk;
+                chan->val_16qam = chan->dup_16qam =
+                        priv->output_limit[i].val_16qam;
+                chan->val_64qam = chan->dup_64qam =
+                        priv->output_limit[i].val_64qam;
                 break;
         }
         if (i == priv->output_limit_len)
                 goto err;
 
-        chan->pa_points_per_curve = priv->curve_data->points_per_channel;
-
         entry = priv->curve_data->data;
         for (i = 0; i < priv->curve_data->channels; i++) {
                 if (*((__le16 *)entry) != freq) {
                         entry += sizeof(__le16);
-                        entry += sizeof(struct pda_pa_curve_data_sample_rev1) *
-                                 chan->pa_points_per_curve;
+                        entry += sizeof(struct p54_pa_curve_data_sample) *
+                                 priv->curve_data->points_per_channel;
                         continue;
                 }
 
                 entry += sizeof(__le16);
+                chan->pa_points_per_curve =
+                        min(priv->curve_data->points_per_channel, (u8) 8);
+
                 memcpy(chan->curve_data, entry, sizeof(*chan->curve_data) *
                        chan->pa_points_per_curve);
                 break;
         }
 
-        memcpy(hdr->data + payload_len - 4, &chan->val_bpsk, 4);
+        chan->rssical_mul = cpu_to_le16(130);
+        chan->rssical_add = cpu_to_le16(0xfe70);        /* -400 */
 
-        priv->tx(dev, hdr, sizeof(*hdr) + payload_len, 1);
+        priv->tx(dev, hdr, sizeof(*hdr) + sizeof(*chan), 1);
         return 0;
 
  err:
@@ -987,7 +1037,7 @@ static int p54_get_tx_stats(struct ieee80211_hw *dev,
 {
         struct p54_common *priv = dev->priv;
 
-        memcpy(stats, &priv->tx_stats, sizeof(stats[0]) * dev->queues);
+        memcpy(stats, &priv->tx_stats[4], sizeof(stats[0]) * dev->queues);
 
         return 0;
 }
@@ -1025,7 +1075,11 @@ struct ieee80211_hw *p54_init_common(size_t priv_data_len)
         dev->channel_change_time = 1000;        /* TODO: find actual value */
         dev->max_signal = 127;
 
-        priv->tx_stats[0].limit = 5;
+        priv->tx_stats[0].limit = 1;
+        priv->tx_stats[1].limit = 1;
+        priv->tx_stats[2].limit = 1;
+        priv->tx_stats[3].limit = 1;
+        priv->tx_stats[4].limit = 5;
         dev->queues = 1;
 
         dev->extra_tx_headroom = sizeof(struct p54_control_hdr) + 4 +
diff --git a/drivers/net/wireless/p54/p54common.h b/drivers/net/wireless/p54/p54common.h
index 9bc2a1cf4b57..a79c1a146917 100644
--- a/drivers/net/wireless/p54/p54common.h
+++ b/drivers/net/wireless/p54/p54common.h
@@ -89,6 +89,16 @@ struct pda_pa_curve_data_sample_rev1 {
         u8 data_qpsk;
         u8 data_16qam;
         u8 data_64qam;
+} __attribute__ ((packed));
+
+struct p54_pa_curve_data_sample {
+        u8 rf_power;
+        u8 pa_detector;
+        u8 data_barker;
+        u8 data_bpsk;
+        u8 data_qpsk;
+        u8 data_16qam;
+        u8 data_64qam;
         u8 padding;
 } __attribute__ ((packed));
 
@@ -212,8 +222,8 @@ struct p54_tx_control_filter {
 } __attribute__ ((packed));
 
 struct p54_tx_control_channel {
-        __le16 magic1;
-        __le16 magic2;
+        __le16 flags;
+        __le16 dwell;
         u8 padding1[20];
         struct pda_iq_autocal_entry iq_autocal;
         u8 pa_points_per_curve;
@@ -222,8 +232,13 @@ struct p54_tx_control_channel {
         u8 val_qpsk;
         u8 val_16qam;
         u8 val_64qam;
-        struct pda_pa_curve_data_sample_rev1 curve_data[0];
-        /* additional padding/data after curve_data */
+        struct pda_pa_curve_data_sample_rev1 curve_data[8];
+        u8 dup_bpsk;
+        u8 dup_qpsk;
+        u8 dup_16qam;
+        u8 dup_64qam;
+        __le16 rssical_mul;
+        __le16 rssical_add;
 } __attribute__ ((packed));
 
 struct p54_tx_control_led {
diff --git a/drivers/net/wireless/p54/p54pci.c b/drivers/net/wireless/p54/p54pci.c
index ea2dc3d93c4d..e9db4495c626 100644
--- a/drivers/net/wireless/p54/p54pci.c
+++ b/drivers/net/wireless/p54/p54pci.c
@@ -3,6 +3,7 @@
  * Linux device driver for PCI based Prism54
  *
  * Copyright (c) 2006, Michael Wu <flamingice@sourmilk.net>
+ * Copyright (c) 2008, Christian Lamparter <chunkeey@web.de>
  *
  * Based on the islsm (softmac prism54) driver, which is:
  * Copyright 2004-2006 Jean-Baptiste Note <jean-baptiste.note@m4x.org>, et al.
@@ -237,20 +238,22 @@ static int p54p_read_eeprom(struct ieee80211_hw *dev)
         return err;
 }
 
-static void p54p_refill_rx_ring(struct ieee80211_hw *dev)
+static void p54p_refill_rx_ring(struct ieee80211_hw *dev,
+        int ring_index, struct p54p_desc *ring, u32 ring_limit,
+        struct sk_buff **rx_buf)
 {
         struct p54p_priv *priv = dev->priv;
         struct p54p_ring_control *ring_control = priv->ring_control;
-        u32 limit, host_idx, idx;
+        u32 limit, idx, i;
 
-        host_idx = le32_to_cpu(ring_control->host_idx[0]);
-        limit = host_idx;
-        limit -= le32_to_cpu(ring_control->device_idx[0]);
-        limit = ARRAY_SIZE(ring_control->rx_data) - limit;
+        idx = le32_to_cpu(ring_control->host_idx[ring_index]);
+        limit = idx;
+        limit -= le32_to_cpu(ring_control->device_idx[ring_index]);
+        limit = ring_limit - limit;
 
-        idx = host_idx % ARRAY_SIZE(ring_control->rx_data);
+        i = idx % ring_limit;
         while (limit-- > 1) {
-                struct p54p_desc *desc = &ring_control->rx_data[idx];
+                struct p54p_desc *desc = &ring[i];
 
                 if (!desc->host_addr) {
                         struct sk_buff *skb;
@@ -267,16 +270,106 @@ static void p54p_refill_rx_ring(struct ieee80211_hw *dev)
                         desc->device_addr = 0;  // FIXME: necessary?
                         desc->len = cpu_to_le16(MAX_RX_SIZE);
                         desc->flags = 0;
-                        priv->rx_buf[idx] = skb;
+                        rx_buf[i] = skb;
                 }
 
+                i++;
                 idx++;
-                host_idx++;
-                idx %= ARRAY_SIZE(ring_control->rx_data);
+                i %= ring_limit;
         }
 
         wmb();
-        ring_control->host_idx[0] = cpu_to_le32(host_idx);
+        ring_control->host_idx[ring_index] = cpu_to_le32(idx);
+}
+
+static void p54p_check_rx_ring(struct ieee80211_hw *dev, u32 *index,
+        int ring_index, struct p54p_desc *ring, u32 ring_limit,
+        struct sk_buff **rx_buf)
+{
+        struct p54p_priv *priv = dev->priv;
+        struct p54p_ring_control *ring_control = priv->ring_control;
+        struct p54p_desc *desc;
+        u32 idx, i;
+
+        i = (*index) % ring_limit;
+        (*index) = idx = le32_to_cpu(ring_control->device_idx[ring_index]);
+        idx %= ring_limit;
+        while (i != idx) {
+                u16 len;
+                struct sk_buff *skb;
+                desc = &ring[i];
+                len = le16_to_cpu(desc->len);
+                skb = rx_buf[i];
+
+                if (!skb)
+                        continue;
+
+                skb_put(skb, len);
+
+                if (p54_rx(dev, skb)) {
+                        pci_unmap_single(priv->pdev,
+                                         le32_to_cpu(desc->host_addr),
+                                         MAX_RX_SIZE, PCI_DMA_FROMDEVICE);
+                        rx_buf[i] = NULL;
+                        desc->host_addr = 0;
+                } else {
+                        skb_trim(skb, 0);
+                        desc->len = cpu_to_le16(MAX_RX_SIZE);
+                }
+
+                i++;
+                i %= ring_limit;
+        }
+
+        p54p_refill_rx_ring(dev, ring_index, ring, ring_limit, rx_buf);
+}
+
+/* caller must hold priv->lock */
+static void p54p_check_tx_ring(struct ieee80211_hw *dev, u32 *index,
+        int ring_index, struct p54p_desc *ring, u32 ring_limit,
+        void **tx_buf)
+{
+        struct p54p_priv *priv = dev->priv;
+        struct p54p_ring_control *ring_control = priv->ring_control;
+        struct p54p_desc *desc;
+        u32 idx, i;
+
+        i = (*index) % ring_limit;
+        (*index) = idx = le32_to_cpu(ring_control->device_idx[1]);
+        idx %= ring_limit;
+
+        while (i != idx) {
+                desc = &ring[i];
+                kfree(tx_buf[i]);
+                tx_buf[i] = NULL;
+
+                pci_unmap_single(priv->pdev, le32_to_cpu(desc->host_addr),
+                                 le16_to_cpu(desc->len), PCI_DMA_TODEVICE);
+
+                desc->host_addr = 0;
+                desc->device_addr = 0;
+                desc->len = 0;
+                desc->flags = 0;
+
+                i++;
+                i %= ring_limit;
+        }
+}
+
+static void p54p_rx_tasklet(unsigned long dev_id)
+{
+        struct ieee80211_hw *dev = (struct ieee80211_hw *)dev_id;
+        struct p54p_priv *priv = dev->priv;
+        struct p54p_ring_control *ring_control = priv->ring_control;
+
+        p54p_check_rx_ring(dev, &priv->rx_idx_mgmt, 2, ring_control->rx_mgmt,
+                ARRAY_SIZE(ring_control->rx_mgmt), priv->rx_buf_mgmt);
+
+        p54p_check_rx_ring(dev, &priv->rx_idx_data, 0, ring_control->rx_data,
+                ARRAY_SIZE(ring_control->rx_data), priv->rx_buf_data);
+
+        wmb();
+        P54P_WRITE(dev_int, cpu_to_le32(ISL38XX_DEV_INT_UPDATE));
 }
 
 static irqreturn_t p54p_interrupt(int irq, void *dev_id)
@@ -298,65 +391,18 @@ static irqreturn_t p54p_interrupt(int irq, void *dev_id)
         reg &= P54P_READ(int_enable);
 
         if (reg & cpu_to_le32(ISL38XX_INT_IDENT_UPDATE)) {
-                struct p54p_desc *desc;
-                u32 idx, i;
-                i = priv->tx_idx;
-                i %= ARRAY_SIZE(ring_control->tx_data);
-                priv->tx_idx = idx = le32_to_cpu(ring_control->device_idx[1]);
-                idx %= ARRAY_SIZE(ring_control->tx_data);
-
-                while (i != idx) {
-                        desc = &ring_control->tx_data[i];
-                        if (priv->tx_buf[i]) {
-                                kfree(priv->tx_buf[i]);
-                                priv->tx_buf[i] = NULL;
-                        }
-
-                        pci_unmap_single(priv->pdev, le32_to_cpu(desc->host_addr),
-                                         le16_to_cpu(desc->len), PCI_DMA_TODEVICE);
+                p54p_check_tx_ring(dev, &priv->tx_idx_mgmt,
+                                   3, ring_control->tx_mgmt,
+                                   ARRAY_SIZE(ring_control->tx_mgmt),
+                                   priv->tx_buf_mgmt);
 
-                        desc->host_addr = 0;
-                        desc->device_addr = 0;
-                        desc->len = 0;
-                        desc->flags = 0;
+                p54p_check_tx_ring(dev, &priv->tx_idx_data,
+                                   1, ring_control->tx_data,
+                                   ARRAY_SIZE(ring_control->tx_data),
+                                   priv->tx_buf_data);
 
-                        i++;
-                        i %= ARRAY_SIZE(ring_control->tx_data);
-                }
-
-                i = priv->rx_idx;
-                i %= ARRAY_SIZE(ring_control->rx_data);
-                priv->rx_idx = idx = le32_to_cpu(ring_control->device_idx[0]);
-                idx %= ARRAY_SIZE(ring_control->rx_data);
-                while (i != idx) {
-                        u16 len;
-                        struct sk_buff *skb;
-                        desc = &ring_control->rx_data[i];
-                        len = le16_to_cpu(desc->len);
-                        skb = priv->rx_buf[i];
-
-                        skb_put(skb, len);
-
-                        if (p54_rx(dev, skb)) {
-                                pci_unmap_single(priv->pdev,
-                                                 le32_to_cpu(desc->host_addr),
-                                                 MAX_RX_SIZE, PCI_DMA_FROMDEVICE);
-
-                                priv->rx_buf[i] = NULL;
-                                desc->host_addr = 0;
-                        } else {
-                                skb_trim(skb, 0);
-                                desc->len = cpu_to_le16(MAX_RX_SIZE);
-                        }
-
-                        i++;
-                        i %= ARRAY_SIZE(ring_control->rx_data);
-                }
+                tasklet_schedule(&priv->rx_tasklet);
 
-                p54p_refill_rx_ring(dev);
-
-                wmb();
-                P54P_WRITE(dev_int, cpu_to_le32(ISL38XX_DEV_INT_UPDATE));
         } else if (reg & cpu_to_le32(ISL38XX_INT_IDENT_INIT))
                 complete(&priv->boot_comp);
 
@@ -392,7 +438,7 @@ static void p54p_tx(struct ieee80211_hw *dev, struct p54_control_hdr *data,
         ring_control->host_idx[1] = cpu_to_le32(idx + 1);
 
         if (free_on_tx)
-                priv->tx_buf[i] = data;
+                priv->tx_buf_data[i] = data;
 
         spin_unlock_irqrestore(&priv->lock, flags);
 
@@ -420,8 +466,14 @@ static int p54p_open(struct ieee80211_hw *dev)
         }
 
         memset(priv->ring_control, 0, sizeof(*priv->ring_control));
-        priv->rx_idx = priv->tx_idx = 0;
-        p54p_refill_rx_ring(dev);
+        priv->rx_idx_data = priv->tx_idx_data = 0;
+        priv->rx_idx_mgmt = priv->tx_idx_mgmt = 0;
+
+        p54p_refill_rx_ring(dev, 0, priv->ring_control->rx_data,
+                ARRAY_SIZE(priv->ring_control->rx_data), priv->rx_buf_data);
+
+        p54p_refill_rx_ring(dev, 2, priv->ring_control->rx_mgmt,
+                ARRAY_SIZE(priv->ring_control->rx_mgmt), priv->rx_buf_mgmt);
 
         p54p_upload_firmware(dev);
 
@@ -465,6 +517,8 @@ static void p54p_stop(struct ieee80211_hw *dev)
         unsigned int i;
         struct p54p_desc *desc;
 
+        tasklet_kill(&priv->rx_tasklet);
+
         P54P_WRITE(int_enable, cpu_to_le32(0));
         P54P_READ(int_enable);
         udelay(10);
@@ -473,26 +527,51 @@ static void p54p_stop(struct ieee80211_hw *dev)
 
         P54P_WRITE(dev_int, cpu_to_le32(ISL38XX_DEV_INT_RESET));
 
-        for (i = 0; i < ARRAY_SIZE(priv->rx_buf); i++) {
+        for (i = 0; i < ARRAY_SIZE(priv->rx_buf_data); i++) {
                 desc = &ring_control->rx_data[i];
                 if (desc->host_addr)
-                        pci_unmap_single(priv->pdev, le32_to_cpu(desc->host_addr),
+                        pci_unmap_single(priv->pdev,
+                                         le32_to_cpu(desc->host_addr),
                                          MAX_RX_SIZE, PCI_DMA_FROMDEVICE);
-                kfree_skb(priv->rx_buf[i]);
-                priv->rx_buf[i] = NULL;
+                kfree_skb(priv->rx_buf_data[i]);
+                priv->rx_buf_data[i] = NULL;
         }
 
-        for (i = 0; i < ARRAY_SIZE(priv->tx_buf); i++) {
+        for (i = 0; i < ARRAY_SIZE(priv->rx_buf_mgmt); i++) {
+                desc = &ring_control->rx_mgmt[i];
+                if (desc->host_addr)
+                        pci_unmap_single(priv->pdev,
+                                         le32_to_cpu(desc->host_addr),
+                                         MAX_RX_SIZE, PCI_DMA_FROMDEVICE);
+                kfree_skb(priv->rx_buf_mgmt[i]);
+                priv->rx_buf_mgmt[i] = NULL;
+        }
+
+        for (i = 0; i < ARRAY_SIZE(priv->tx_buf_data); i++) {
                 desc = &ring_control->tx_data[i];
                 if (desc->host_addr)
-                        pci_unmap_single(priv->pdev, le32_to_cpu(desc->host_addr),
-                                         le16_to_cpu(desc->len), PCI_DMA_TODEVICE);
+                        pci_unmap_single(priv->pdev,
+                                         le32_to_cpu(desc->host_addr),
+                                         le16_to_cpu(desc->len),
+                                         PCI_DMA_TODEVICE);
+
+                kfree(priv->tx_buf_data[i]);
+                priv->tx_buf_data[i] = NULL;
+        }
+
+        for (i = 0; i < ARRAY_SIZE(priv->tx_buf_mgmt); i++) {
+                desc = &ring_control->tx_mgmt[i];
+                if (desc->host_addr)
+                        pci_unmap_single(priv->pdev,
+                                         le32_to_cpu(desc->host_addr),
+                                         le16_to_cpu(desc->len),
+                                         PCI_DMA_TODEVICE);
 
-                kfree(priv->tx_buf[i]);
-                priv->tx_buf[i] = NULL;
+                kfree(priv->tx_buf_mgmt[i]);
+                priv->tx_buf_mgmt[i] = NULL;
         }
 
-        memset(ring_control, 0, sizeof(ring_control));
+        memset(ring_control, 0, sizeof(*ring_control));
 }
 
 static int __devinit p54p_probe(struct pci_dev *pdev,
@@ -585,6 +664,7 @@ static int __devinit p54p_probe(struct pci_dev *pdev,
         priv->common.tx = p54p_tx;
 
         spin_lock_init(&priv->lock);
+        tasklet_init(&priv->rx_tasklet, p54p_rx_tasklet, (unsigned long)dev);
 
         err = ieee80211_register_hw(dev);
         if (err) {
diff --git a/drivers/net/wireless/p54/p54pci.h b/drivers/net/wireless/p54/p54pci.h
index 07678ef5ddc8..4a6778070afc 100644
--- a/drivers/net/wireless/p54/p54pci.h
+++ b/drivers/net/wireless/p54/p54pci.h
@@ -92,13 +92,17 @@ struct p54p_priv {
         struct p54_common common;
         struct pci_dev *pdev;
         struct p54p_csr __iomem *map;
+        struct tasklet_struct rx_tasklet;
 
         spinlock_t lock;
         struct p54p_ring_control *ring_control;
         dma_addr_t ring_control_dma;
-        u32 rx_idx, tx_idx;
-        struct sk_buff *rx_buf[8];
-        void *tx_buf[32];
+        u32 rx_idx_data, tx_idx_data;
+        u32 rx_idx_mgmt, tx_idx_mgmt;
+        struct sk_buff *rx_buf_data[8];
+        struct sk_buff *rx_buf_mgmt[4];
+        void *tx_buf_data[32];
+        void *tx_buf_mgmt[4];
         struct completion boot_comp;
 };