b43legacy: fix upload of beacon packets to the hardware

This fixes uploading of the beacon data and writing of the TIM and DTIM offsets.

The patch by Michael Buesch has been ported to b43legacy.

Signed-off-by: Stefano Brivio <stefano.brivio@polimi.it>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

1 files changed, 160 insertions, 97 deletions

diff --git a/drivers/net/wireless/b43legacy/main.c b/drivers/net/wireless/b43legacy/main.c
index 9db949ed651..62e679a1c05 100644
--- a/drivers/net/wireless/b43legacy/main.c
+++ b/drivers/net/wireless/b43legacy/main.c
@@ -962,16 +962,61 @@ static void b43legacy_write_beacon_template(struct b43legacy_wldev *dev,
 					    u16 ram_offset,
 					    u16 shm_size_offset, u8 rate)
 {
-	int len;
-	const u8 *data;
 
-	B43legacy_WARN_ON(!dev->cached_beacon);
-	len = min((size_t)dev->cached_beacon->len,
+	unsigned int i, len, variable_len;
+	const struct ieee80211_mgmt *bcn;
+	const u8 *ie;
+	bool tim_found = 0;
+
+	bcn = (const struct ieee80211_mgmt *)(dev->wl->current_beacon->data);
+	len = min((size_t)dev->wl->current_beacon->len,
 		  0x200 - sizeof(struct b43legacy_plcp_hdr6));
-	data = (const u8 *)(dev->cached_beacon->data);
-	b43legacy_write_template_common(dev, data,
-					len, ram_offset,
+
+	b43legacy_write_template_common(dev, (const u8 *)bcn, len, ram_offset,
 					shm_size_offset, rate);
+
+	/* Find the position of the TIM and the DTIM_period value
+	 * and write them to SHM. */
+	ie = bcn->u.beacon.variable;
+	variable_len = len - offsetof(struct ieee80211_mgmt, u.beacon.variable);
+	for (i = 0; i < variable_len - 2; ) {
+		uint8_t ie_id, ie_len;
+
+		ie_id = ie[i];
+		ie_len = ie[i + 1];
+		if (ie_id == 5) {
+			u16 tim_position;
+			u16 dtim_period;
+			/* This is the TIM Information Element */
+
+			/* Check whether the ie_len is in the beacon data range. */
+			if (variable_len < ie_len + 2 + i)
+				break;
+			/* A valid TIM is at least 4 bytes long. */
+			if (ie_len < 4)
+				break;
+			tim_found = 1;
+
+			tim_position = sizeof(struct b43legacy_plcp_hdr6);
+			tim_position += offsetof(struct ieee80211_mgmt,
+						 u.beacon.variable);
+			tim_position += i;
+
+			dtim_period = ie[i + 3];
+
+			b43legacy_shm_write16(dev, B43legacy_SHM_SHARED,
+					B43legacy_SHM_SH_TIMPOS, tim_position);
+			b43legacy_shm_write16(dev, B43legacy_SHM_SHARED,
+					B43legacy_SHM_SH_DTIMP, dtim_period);
+			break;
+		}
+		i += ie_len + 2;
+	}
+	if (!tim_found) {
+		b43legacywarn(dev->wl, "Did not find a valid TIM IE in the "
+			      "beacon template packet. AP or IBSS operation "
+			      "may be broken.\n");
+	}
 }
 
 static void b43legacy_write_probe_resp_plcp(struct b43legacy_wldev *dev,
@@ -1004,26 +1049,27 @@ static void b43legacy_write_probe_resp_plcp(struct b43legacy_wldev *dev,
  * 2) Patching duration field
  * 3) Stripping TIM
  */
-static u8 *b43legacy_generate_probe_resp(struct b43legacy_wldev *dev,
-					 u16 *dest_size,
-					 struct ieee80211_rate *rate)
+static const u8 *b43legacy_generate_probe_resp(struct b43legacy_wldev *dev,
+					       u16 *dest_size,
+					       struct ieee80211_rate *rate)
 {
 	const u8 *src_data;
 	u8 *dest_data;
-	u16 src_size;
-	u16 elem_size;
-	u16 src_pos;
-	u16 dest_pos;
+	u16 src_size, elem_size, src_pos, dest_pos;
 	__le16 dur;
 	struct ieee80211_hdr *hdr;
+	size_t ie_start;
+
+	src_size = dev->wl->current_beacon->len;
+	src_data = (const u8 *)dev->wl->current_beacon->data;
 
-	B43legacy_WARN_ON(!dev->cached_beacon);
-	src_size = dev->cached_beacon->len;
-	src_data = (const u8 *)dev->cached_beacon->data;
+	/* Get the start offset of the variable IEs in the packet. */
+	ie_start = offsetof(struct ieee80211_mgmt, u.probe_resp.variable);
+	B43legacy_WARN_ON(ie_start != offsetof(struct ieee80211_mgmt,
+					       u.beacon.variable));
 
-	if (unlikely(src_size < 0x24)) {
-		b43legacydbg(dev->wl, "b43legacy_generate_probe_resp: "
-		       "invalid beacon\n");
+	if (src_size < ie_start) {
+		B43legacy_WARN_ON(1);
 		return NULL;
 	}
 
@@ -1031,19 +1077,18 @@ static u8 *b43legacy_generate_probe_resp(struct b43legacy_wldev *dev,
 	if (unlikely(!dest_data))
 		return NULL;
 
-	/* 0x24 is offset of first variable-len Information-Element
-	 * in beacon frame.
-	 */
-	memcpy(dest_data, src_data, 0x24);
-	src_pos = 0x24;
-	dest_pos = 0x24;
-	for (; src_pos < src_size - 2; src_pos += elem_size) {
+	/* Copy the static data and all Information Elements, except the TIM. */
+	memcpy(dest_data, src_data, ie_start);
+	src_pos = ie_start;
+	dest_pos = ie_start;
+	for ( ; src_pos < src_size - 2; src_pos += elem_size) {
 		elem_size = src_data[src_pos + 1] + 2;
-		if (src_data[src_pos] != 0x05) { /* TIM */
-			memcpy(dest_data + dest_pos, src_data + src_pos,
-			       elem_size);
-			dest_pos += elem_size;
+		if (src_data[src_pos] == 5) {
+			/* This is the TIM. */
+			continue;
 		}
+		memcpy(dest_data + dest_pos, src_data + src_pos, elem_size);
+		dest_pos += elem_size;
 	}
 	*dest_size = dest_pos;
 	hdr = (struct ieee80211_hdr *)dest_data;
@@ -1065,11 +1110,10 @@ static void b43legacy_write_probe_resp_template(struct b43legacy_wldev *dev,
 						u16 shm_size_offset,
 						struct ieee80211_rate *rate)
 {
-	u8 *probe_resp_data;
+	const u8 *probe_resp_data;
 	u16 size;
 
-	B43legacy_WARN_ON(!dev->cached_beacon);
-	size = dev->cached_beacon->len;
+	size = dev->wl->current_beacon->len;
 	probe_resp_data = b43legacy_generate_probe_resp(dev, &size, rate);
 	if (unlikely(!probe_resp_data))
 		return;
@@ -1094,43 +1138,21 @@ static void b43legacy_write_probe_resp_template(struct b43legacy_wldev *dev,
 	kfree(probe_resp_data);
 }
 
-static int b43legacy_refresh_cached_beacon(struct b43legacy_wldev *dev,
-					   struct sk_buff *beacon)
+/* Asynchronously update the packet templates in template RAM.
+ * Locking: Requires wl->irq_lock to be locked. */
+static void b43legacy_update_templates(struct b43legacy_wl *wl,
+				       struct sk_buff *beacon)
 {
-	if (dev->cached_beacon)
-		kfree_skb(dev->cached_beacon);
-	dev->cached_beacon = beacon;
+	/* This is the top half of the ansynchronous beacon update. The bottom
+	 * half is the beacon IRQ. Beacon update must be asynchronous to avoid
+	 * sending an invalid beacon. This can happen for example, if the
+	 * firmware transmits a beacon while we are updating it. */
 
-	return 0;
-}
-
-static void b43legacy_update_templates(struct b43legacy_wldev *dev)
-{
-	u32 cmd;
-
-	B43legacy_WARN_ON(!dev->cached_beacon);
-
-	b43legacy_write_beacon_template(dev, 0x68, 0x18,
-					B43legacy_CCK_RATE_1MB);
-	b43legacy_write_beacon_template(dev, 0x468, 0x1A,
-					B43legacy_CCK_RATE_1MB);
-	b43legacy_write_probe_resp_template(dev, 0x268, 0x4A,
-					    &b43legacy_b_ratetable[0]);
-
-	cmd = b43legacy_read32(dev, B43legacy_MMIO_MACCMD);
-	cmd |= B43legacy_MACCMD_BEACON0_VALID | B43legacy_MACCMD_BEACON1_VALID;
-	b43legacy_write32(dev, B43legacy_MMIO_MACCMD, cmd);
-}
-
-static void b43legacy_refresh_templates(struct b43legacy_wldev *dev,
-					struct sk_buff *beacon)
-{
-	int err;
-
-	err = b43legacy_refresh_cached_beacon(dev, beacon);
-	if (unlikely(err))
-		return;
-	b43legacy_update_templates(dev);
+	if (wl->current_beacon)
+		dev_kfree_skb_any(wl->current_beacon);
+	wl->current_beacon = beacon;
+	wl->beacon0_uploaded = 0;
+	wl->beacon1_uploaded = 0;
 }
 
 static void b43legacy_set_ssid(struct b43legacy_wldev *dev,
@@ -1171,38 +1193,37 @@ static void b43legacy_set_beacon_int(struct b43legacy_wldev *dev,
 
 static void handle_irq_beacon(struct b43legacy_wldev *dev)
 {
-	u32 status;
+	struct b43legacy_wl *wl = dev->wl;
+	u32 cmd;
 
-	if (!b43legacy_is_mode(dev->wl, IEEE80211_IF_TYPE_AP))
+	if (!b43legacy_is_mode(wl, IEEE80211_IF_TYPE_AP))
 		return;
 
-	dev->irq_savedstate &= ~B43legacy_IRQ_BEACON;
-	status = b43legacy_read32(dev, B43legacy_MMIO_MACCMD);
+	/* This is the bottom half of the asynchronous beacon update. */
 
-	if (!dev->cached_beacon || ((status & 0x1) && (status & 0x2))) {
-		/* ACK beacon IRQ. */
-		b43legacy_write32(dev, B43legacy_MMIO_GEN_IRQ_REASON,
-				  B43legacy_IRQ_BEACON);
-		dev->irq_savedstate |= B43legacy_IRQ_BEACON;
-		if (dev->cached_beacon)
-			kfree_skb(dev->cached_beacon);
-		dev->cached_beacon = NULL;
-		return;
-	}
-	if (!(status & 0x1)) {
-		b43legacy_write_beacon_template(dev, 0x68, 0x18,
-						B43legacy_CCK_RATE_1MB);
-		status |= 0x1;
-		b43legacy_write32(dev, B43legacy_MMIO_MACCMD,
-				  status);
-	}
-	if (!(status & 0x2)) {
-		b43legacy_write_beacon_template(dev, 0x468, 0x1A,
-						B43legacy_CCK_RATE_1MB);
-		status |= 0x2;
-		b43legacy_write32(dev, B43legacy_MMIO_MACCMD,
-				  status);
+	cmd = b43legacy_read32(dev, B43legacy_MMIO_MACCMD);
+	if (!(cmd & B43legacy_MACCMD_BEACON0_VALID)) {
+		if (!wl->beacon0_uploaded) {
+			b43legacy_write_beacon_template(dev, 0x68,
+							B43legacy_SHM_SH_BTL0,
+							B43legacy_CCK_RATE_1MB);
+			b43legacy_write_probe_resp_template(dev, 0x268,
+							    B43legacy_SHM_SH_PRTLEN,
+							    &__b43legacy_ratetable[3]);
+			wl->beacon0_uploaded = 1;
+		}
+		cmd |= B43legacy_MACCMD_BEACON0_VALID;
+	}
+	if (!(cmd & B43legacy_MACCMD_BEACON1_VALID)) {
+		if (!wl->beacon1_uploaded) {
+			b43legacy_write_beacon_template(dev, 0x468,
+							B43legacy_SHM_SH_BTL1,
+							B43legacy_CCK_RATE_1MB);
+			wl->beacon1_uploaded = 1;
+		}
+		cmd |= B43legacy_MACCMD_BEACON1_VALID;
 	}
+	b43legacy_write32(dev, B43legacy_MMIO_MACCMD, cmd);
 }
 
 static void handle_irq_ucode_debug(struct b43legacy_wldev *dev)
@@ -2709,7 +2730,7 @@ static int b43legacy_op_config_interface(struct ieee80211_hw *hw,
 			B43legacy_WARN_ON(conf->type != IEEE80211_IF_TYPE_AP);
 			b43legacy_set_ssid(dev, conf->ssid, conf->ssid_len);
 			if (conf->beacon)
-				b43legacy_refresh_templates(dev, conf->beacon);
+				b43legacy_update_templates(wl, conf->beacon);
 		}
 		b43legacy_write_mac_bssid_templates(dev);
 	}
@@ -3022,6 +3043,11 @@ static void b43legacy_wireless_core_exit(struct b43legacy_wldev *dev)
 		kfree(phy->tssi2dbm);
 	kfree(phy->lo_control);
 	phy->lo_control = NULL;
+	if (dev->wl->current_beacon) {
+		dev_kfree_skb_any(dev->wl->current_beacon);
+		dev->wl->current_beacon = NULL;
+	}
+
 	ssb_device_disable(dev->dev, 0);
 	ssb_bus_may_powerdown(dev->dev->bus);
 }
@@ -3346,6 +3372,41 @@ out_unlock:
 	return err;
 }
 
+static int b43legacy_op_beacon_set_tim(struct ieee80211_hw *hw,
+				       int aid, int set)
+{
+	struct b43legacy_wl *wl = hw_to_b43legacy_wl(hw);
+	struct sk_buff *beacon;
+	unsigned long flags;
+
+	/* We could modify the existing beacon and set the aid bit in the TIM
+	 * field, but that would probably require resizing and moving of data
+	 * within the beacon template. Simply request a new beacon and let
+	 * mac80211 do the hard work. */
+	beacon = ieee80211_beacon_get(hw, wl->vif, NULL);
+	if (unlikely(!beacon))
+		return -ENOMEM;
+	spin_lock_irqsave(&wl->irq_lock, flags);
+	b43legacy_update_templates(wl, beacon);
+	spin_unlock_irqrestore(&wl->irq_lock, flags);
+
+	return 0;
+}
+
+static int b43legacy_op_ibss_beacon_update(struct ieee80211_hw *hw,
+					   struct sk_buff *beacon,
+					   struct ieee80211_tx_control *ctl)
+{
+	struct b43legacy_wl *wl = hw_to_b43legacy_wl(hw);
+	unsigned long flags;
+
+	spin_lock_irqsave(&wl->irq_lock, flags);
+	b43legacy_update_templates(wl, beacon);
+	spin_unlock_irqrestore(&wl->irq_lock, flags);
+
+	return 0;
+}
+
 static const struct ieee80211_ops b43legacy_hw_ops = {
 	.tx			= b43legacy_op_tx,
 	.conf_tx		= b43legacy_op_conf_tx,
@@ -3359,6 +3420,8 @@ static const struct ieee80211_ops b43legacy_hw_ops = {
 	.start			= b43legacy_op_start,
 	.stop			= b43legacy_op_stop,
 	.set_retry_limit	= b43legacy_op_set_retry_limit,
+	.set_tim		= b43legacy_op_beacon_set_tim,
+	.beacon_update		= b43legacy_op_ibss_beacon_update,
 };
 
 /* Hard-reset the chip. Do not call this directly.