b43: Put multicast frames on the mcast queue

This queues frames flagged as "send after DTIM" by mac80211
on the special multicast queue. The firmware will take care
to send the packet after the DTIM.
Signed-off-by: Michael Buesch <mb@bu3sch.de>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

drivers/net/wireless/b43/b43.h

@@ -170,14 +170,17 @@ enum {
 #define B43_SHM_SH_SLOTT		0x0010	/* Slot time */
 #define B43_SHM_SH_DTIMPER		0x0012	/* DTIM period */
 #define B43_SHM_SH_NOSLPZNATDTIM	0x004C	/* NOSLPZNAT DTIM */
-/* SHM_SHARED beacon variables */
+/* SHM_SHARED beacon/AP variables */
 #define B43_SHM_SH_BTL0			0x0018	/* Beacon template length 0 */
 #define B43_SHM_SH_BTL1			0x001A	/* Beacon template length 1 */
 #define B43_SHM_SH_BTSFOFF		0x001C	/* Beacon TSF offset */
 #define B43_SHM_SH_TIMBPOS		0x001E	/* TIM B position in beacon */
+#define B43_SHM_SH_DTIMP		0x0012	/* DTIP period */
+#define B43_SHM_SH_MCASTCOOKIE		0x00A8	/* Last bcast/mcast frame ID */
 #define B43_SHM_SH_SFFBLIM		0x0044	/* Short frame fallback retry limit */
 #define B43_SHM_SH_LFFBLIM		0x0046	/* Long frame fallback retry limit */
 #define B43_SHM_SH_BEACPHYCTL		0x0054	/* Beacon PHY TX control word (see PHY TX control) */
+#define B43_SHM_SH_EXTNPHYCTL		0x00B0	/* Extended bytes for beacon PHY control (N) */
 /* SHM_SHARED ACK/CTS control */
 #define B43_SHM_SH_ACKCTSPHYCTL		0x0022	/* ACK/CTS PHY control word (see PHY TX control) */
 /* SHM_SHARED probe response variables */
@@ -617,9 +620,12 @@ struct b43_wl {
 	/* Pointer to the ieee80211 hardware data structure */
 	struct ieee80211_hw *hw;
 
-	spinlock_t irq_lock;
 	struct mutex mutex;
+	spinlock_t irq_lock;
+	/* Lock for LEDs access. */
 	spinlock_t leds_lock;
+	/* Lock for SHM access. */
+	spinlock_t shm_lock;
 
 	/* We can only have one operating interface (802.11 core)
 	 * at a time. General information about this interface follows.

drivers/net/wireless/b43/dma.c

@@ -37,6 +37,8 @@
 #include <linux/pci.h>
 #include <linux/delay.h>
 #include <linux/skbuff.h>
+#include <linux/etherdevice.h>
+
 
 /* 32bit DMA ops. */
 static
@@ -315,26 +317,24 @@ static struct b43_dmaring *priority_to_txring(struct b43_wldev *dev,
 	case 3:
 		ring = dev->dma.tx_ring0;
 		break;
-	case 4:
-		ring = dev->dma.tx_ring4;
-		break;
-	case 5:
-		ring = dev->dma.tx_ring5;
-		break;
 	}
 
 	return ring;
 }
 
-/* Bcm43xx-ring to mac80211-queue mapping */
+/* b43-ring to mac80211-queue mapping */
 static inline int txring_to_priority(struct b43_dmaring *ring)
 {
-	static const u8 idx_to_prio[] = { 3, 2, 1, 0, 4, 5, };
+	static const u8 idx_to_prio[] = { 3, 2, 1, 0, };
+	unsigned int index;
 
 /*FIXME: have only one queue, for now */
 	return 0;
 
-	return idx_to_prio[ring->index];
+	index = ring->index;
+	if (B43_WARN_ON(index >= ARRAY_SIZE(idx_to_prio)))
+		index = 0;
+	return idx_to_prio[index];
 }
 
 u16 b43_dmacontroller_base(int dma64bit, int controller_idx)
@@ -1043,26 +1043,30 @@ static u16 generate_cookie(struct b43_dmaring *ring, int slot)
 	 * in the lower 12 bits.
 	 * Note that the cookie must never be 0, as this
 	 * is a special value used in RX path.
+	 * It can also not be 0xFFFF because that is special
+	 * for multicast frames.
 	 */
 	switch (ring->index) {
 	case 0:
-		cookie = 0xA000;
+		cookie = 0x1000;
 		break;
 	case 1:
-		cookie = 0xB000;
+		cookie = 0x2000;
 		break;
 	case 2:
-		cookie = 0xC000;
+		cookie = 0x3000;
 		break;
 	case 3:
-		cookie = 0xD000;
+		cookie = 0x4000;
 		break;
 	case 4:
-		cookie = 0xE000;
+		cookie = 0x5000;
 		break;
 	case 5:
-		cookie = 0xF000;
+		cookie = 0x6000;
 		break;
+	default:
+		B43_WARN_ON(1);
 	}
 	B43_WARN_ON(slot & ~0x0FFF);
 	cookie |= (u16) slot;
@@ -1078,22 +1082,22 @@ struct b43_dmaring *parse_cookie(struct b43_wldev *dev, u16 cookie, int *slot)
 	struct b43_dmaring *ring = NULL;
 
 	switch (cookie & 0xF000) {
-	case 0xA000:
+	case 0x1000:
 		ring = dma->tx_ring0;
 		break;
-	case 0xB000:
+	case 0x2000:
 		ring = dma->tx_ring1;
 		break;
-	case 0xC000:
+	case 0x3000:
 		ring = dma->tx_ring2;
 		break;
-	case 0xD000:
+	case 0x4000:
 		ring = dma->tx_ring3;
 		break;
-	case 0xE000:
+	case 0x5000:
 		ring = dma->tx_ring4;
 		break;
-	case 0xF000:
+	case 0x6000:
 		ring = dma->tx_ring5;
 		break;
 	default:
@@ -1117,6 +1121,7 @@ static int dma_tx_fragment(struct b43_dmaring *ring,
 	struct b43_dmadesc_meta *meta;
 	struct b43_dmadesc_meta *meta_hdr;
 	struct sk_buff *bounce_skb;
+	u16 cookie;
 
 #define SLOTS_PER_PACKET  2
 	B43_WARN_ON(skb_shinfo(skb)->nr_frags);
@@ -1127,9 +1132,9 @@ static int dma_tx_fragment(struct b43_dmaring *ring,
 	memset(meta_hdr, 0, sizeof(*meta_hdr));
 
 	header = &(ring->txhdr_cache[slot * sizeof(struct b43_txhdr_fw4)]);
+	cookie = generate_cookie(ring, slot);
 	b43_generate_txhdr(ring->dev, header,
-			   skb->data, skb->len, ctl,
-			   generate_cookie(ring, slot));
+			   skb->data, skb->len, ctl, cookie);
 
 	meta_hdr->dmaaddr = map_descbuffer(ring, (unsigned char *)header,
 					   sizeof(struct b43_txhdr_fw4), 1);
@@ -1169,14 +1174,20 @@ static int dma_tx_fragment(struct b43_dmaring *ring,
 
 	ops->fill_descriptor(ring, desc, meta->dmaaddr, skb->len, 0, 1, 1);
 
+	if (ctl->flags & IEEE80211_TXCTL_SEND_AFTER_DTIM) {
+		/* Tell the firmware about the cookie of the last
+		 * mcast frame, so it can clear the more-data bit in it. */
+		b43_shm_write16(ring->dev, B43_SHM_SHARED,
+				B43_SHM_SH_MCASTCOOKIE, cookie);
+	}
 	/* Now transfer the whole frame. */
 	wmb();
 	ops->poke_tx(ring, next_slot(ring, slot));
 	return 0;
 
-      out_free_bounce:
+out_free_bounce:
 	dev_kfree_skb_any(skb);
-      out_unmap_hdr:
+out_unmap_hdr:
 	unmap_descbuffer(ring, meta_hdr->dmaaddr,
 			 sizeof(struct b43_txhdr_fw4), 1);
 	return err;
@@ -1207,10 +1218,27 @@ int b43_dma_tx(struct b43_wldev *dev,
 	       struct sk_buff *skb, struct ieee80211_tx_control *ctl)
 {
 	struct b43_dmaring *ring;
+	struct ieee80211_hdr *hdr;
 	int err = 0;
 	unsigned long flags;
 
+	if (unlikely(skb->len < 2 + 2 + 6)) {
+		/* Too short, this can't be a valid frame. */
+		return -EINVAL;
+	}
+
+	hdr = (struct ieee80211_hdr *)skb->data;
+	if (ctl->flags & IEEE80211_TXCTL_SEND_AFTER_DTIM) {
+		/* The multicast ring will be sent after the DTIM */
+		ring = dev->dma.tx_ring4;
+		/* Set the more-data bit. Ucode will clear it on
+		 * the last frame for us. */
+		hdr->frame_control |= cpu_to_le16(IEEE80211_FCTL_MOREDATA);
+	} else {
+		/* Decide by priority where to put this frame. */
 		ring = priority_to_txring(dev, ctl->queue);
+	}
+
 	spin_lock_irqsave(&ring->lock, flags);
 	B43_WARN_ON(!ring->tx);
 	if (unlikely(free_slots(ring) < SLOTS_PER_PACKET)) {
@@ -1238,7 +1266,7 @@ int b43_dma_tx(struct b43_wldev *dev,
 			b43dbg(dev->wl, "Stopped TX ring %d\n", ring->index);
 		}
 	}
-      out_unlock:
+out_unlock:
 	spin_unlock_irqrestore(&ring->lock, flags);
 
 	return err;

drivers/net/wireless/b43/main.c

@@ -252,13 +252,12 @@ static void b43_ram_write(struct b43_wldev *dev, u16 offset, u32 val)
 	b43_write32(dev, B43_MMIO_RAM_DATA, val);
 }
 
-static inline
-    void b43_shm_control_word(struct b43_wldev *dev, u16 routing, u16 offset)
+static inline void b43_shm_control_word(struct b43_wldev *dev,
+					u16 routing, u16 offset)
 {
 	u32 control;
 
 	/* "offset" is the WORD offset. */
-
 	control = routing;
 	control <<= 16;
 	control |= offset;
@@ -267,8 +266,11 @@ static inline
 
 u32 b43_shm_read32(struct b43_wldev *dev, u16 routing, u16 offset)
 {
+	struct b43_wl *wl = dev->wl;
+	unsigned long flags;
 	u32 ret;
 
+	spin_lock_irqsave(&wl->shm_lock, flags);
 	if (routing == B43_SHM_SHARED) {
 		B43_WARN_ON(offset & 0x0001);
 		if (offset & 0x0003) {
@@ -279,20 +281,25 @@ u32 b43_shm_read32(struct b43_wldev *dev, u16 routing, u16 offset)
 			b43_shm_control_word(dev, routing, (offset >> 2) + 1);
 			ret |= b43_read16(dev, B43_MMIO_SHM_DATA);
 
-			return ret;
+			goto out;
 		}
 		offset >>= 2;
 	}
 	b43_shm_control_word(dev, routing, offset);
 	ret = b43_read32(dev, B43_MMIO_SHM_DATA);
+out:
+	spin_unlock_irqrestore(&wl->shm_lock, flags);
 
 	return ret;
 }
 
 u16 b43_shm_read16(struct b43_wldev * dev, u16 routing, u16 offset)
 {
+	struct b43_wl *wl = dev->wl;
+	unsigned long flags;
 	u16 ret;
 
+	spin_lock_irqsave(&wl->shm_lock, flags);
 	if (routing == B43_SHM_SHARED) {
 		B43_WARN_ON(offset & 0x0001);
 		if (offset & 0x0003) {
@@ -300,55 +307,63 @@ u16 b43_shm_read16(struct b43_wldev * dev, u16 routing, u16 offset)
 			b43_shm_control_word(dev, routing, offset >> 2);
 			ret = b43_read16(dev, B43_MMIO_SHM_DATA_UNALIGNED);
 
-			return ret;
+			goto out;
 		}
 		offset >>= 2;
 	}
 	b43_shm_control_word(dev, routing, offset);
 	ret = b43_read16(dev, B43_MMIO_SHM_DATA);
+out:
+	spin_unlock_irqrestore(&wl->shm_lock, flags);
 
 	return ret;
 }
 
 void b43_shm_write32(struct b43_wldev *dev, u16 routing, u16 offset, u32 value)
 {
+	struct b43_wl *wl = dev->wl;
+	unsigned long flags;
+
+	spin_lock_irqsave(&wl->shm_lock, flags);
 	if (routing == B43_SHM_SHARED) {
 		B43_WARN_ON(offset & 0x0001);
 		if (offset & 0x0003) {
 			/* Unaligned access */
 			b43_shm_control_word(dev, routing, offset >> 2);
-			mmiowb();
 			b43_write16(dev, B43_MMIO_SHM_DATA_UNALIGNED,
 				    (value >> 16) & 0xffff);
-			mmiowb();
 			b43_shm_control_word(dev, routing, (offset >> 2) + 1);
-			mmiowb();
 			b43_write16(dev, B43_MMIO_SHM_DATA, value & 0xffff);
-			return;
+			goto out;
 		}
 		offset >>= 2;
 	}
 	b43_shm_control_word(dev, routing, offset);
-	mmiowb();
 	b43_write32(dev, B43_MMIO_SHM_DATA, value);
+out:
+	spin_unlock_irqrestore(&wl->shm_lock, flags);
 }
 
 void b43_shm_write16(struct b43_wldev *dev, u16 routing, u16 offset, u16 value)
 {
+	struct b43_wl *wl = dev->wl;
+	unsigned long flags;
+
+	spin_lock_irqsave(&wl->shm_lock, flags);
 	if (routing == B43_SHM_SHARED) {
 		B43_WARN_ON(offset & 0x0001);
 		if (offset & 0x0003) {
 			/* Unaligned access */
 			b43_shm_control_word(dev, routing, offset >> 2);
-			mmiowb();
 			b43_write16(dev, B43_MMIO_SHM_DATA_UNALIGNED, value);
-			return;
+			goto out;
 		}
 		offset >>= 2;
 	}
 	b43_shm_control_word(dev, routing, offset);
-	mmiowb();
 	b43_write16(dev, B43_MMIO_SHM_DATA, value);
+out:
+	spin_unlock_irqrestore(&wl->shm_lock, flags);
 }
 
 /* Read HostFlags */
@@ -3931,6 +3946,7 @@ static int b43_wireless_init(struct ssb_device *dev)
 	wl->hw = hw;
 	spin_lock_init(&wl->irq_lock);
 	spin_lock_init(&wl->leds_lock);
+	spin_lock_init(&wl->shm_lock);
 	mutex_init(&wl->mutex);
 	INIT_LIST_HEAD(&wl->devlist);