b43: Fix PHY TX control words in SHM

This fixes the initialization of the PHY TX control words in
shared memory. These control words are used for management frames
like beacons.

Signed-off-by: Michael Buesch <mb@bu3sch.de>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

2 files changed, 117 insertions, 83 deletions

diff --git a/drivers/net/wireless/b43/b43.h b/drivers/net/wireless/b43/b43.h
index 4ba4a73673bc..eff2a158a411 100644
--- a/drivers/net/wireless/b43/b43.h
+++ b/drivers/net/wireless/b43/b43.h
@@ -730,6 +730,7 @@ struct b43_wl {
         /* The beacon we are currently using (AP or IBSS mode).
          * This beacon stuff is protected by the irq_lock. */
         struct sk_buff *current_beacon;
+        struct ieee80211_tx_control beacon_txctl;
         bool beacon0_uploaded;
         bool beacon1_uploaded;
         struct work_struct beacon_update_trigger;
diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/b43/main.c
index ec28a6ef42be..1aa0f727174b 100644
--- a/drivers/net/wireless/b43/main.c
+++ b/drivers/net/wireless/b43/main.c
@@ -1281,22 +1281,107 @@ static void b43_write_template_common(struct b43_wldev *dev,
                         size + sizeof(struct b43_plcp_hdr6));
 }
 
+/* Check if the use of the antenna that ieee80211 told us to
+ * use is possible. This will fall back to DEFAULT.
+ * "antenna_nr" is the antenna identifier we got from ieee80211. */
+u8 b43_ieee80211_antenna_sanitize(struct b43_wldev *dev,
+                                  u8 antenna_nr)
+{
+        u8 antenna_mask;
+
+        if (antenna_nr == 0) {
+                /* Zero means "use default antenna". That's always OK. */
+                return 0;
+        }
+
+        /* Get the mask of available antennas. */
+        if (dev->phy.gmode)
+                antenna_mask = dev->dev->bus->sprom.ant_available_bg;
+        else
+                antenna_mask = dev->dev->bus->sprom.ant_available_a;
+
+        if (!(antenna_mask & (1 << (antenna_nr - 1)))) {
+                /* This antenna is not available. Fall back to default. */
+                return 0;
+        }
+
+        return antenna_nr;
+}
+
+static int b43_antenna_from_ieee80211(struct b43_wldev *dev, u8 antenna)
+{
+        antenna = b43_ieee80211_antenna_sanitize(dev, antenna);
+        switch (antenna) {
+        case 0:         /* default/diversity */
+                return B43_ANTENNA_DEFAULT;
+        case 1:         /* Antenna 0 */
+                return B43_ANTENNA0;
+        case 2:         /* Antenna 1 */
+                return B43_ANTENNA1;
+        case 3:         /* Antenna 2 */
+                return B43_ANTENNA2;
+        case 4:         /* Antenna 3 */
+                return B43_ANTENNA3;
+        default:
+                return B43_ANTENNA_DEFAULT;
+        }
+}
+
+/* Convert a b43 antenna number value to the PHY TX control value. */
+static u16 b43_antenna_to_phyctl(int antenna)
+{
+        switch (antenna) {
+        case B43_ANTENNA0:
+                return B43_TXH_PHY_ANT0;
+        case B43_ANTENNA1:
+                return B43_TXH_PHY_ANT1;
+        case B43_ANTENNA2:
+                return B43_TXH_PHY_ANT2;
+        case B43_ANTENNA3:
+                return B43_TXH_PHY_ANT3;
+        case B43_ANTENNA_AUTO:
+                return B43_TXH_PHY_ANT01AUTO;
+        }
+        B43_WARN_ON(1);
+        return 0;
+}
+
 static void b43_write_beacon_template(struct b43_wldev *dev,
                                       u16 ram_offset,
-                                      u16 shm_size_offset, u8 rate)
+                                      u16 shm_size_offset)
 {
         unsigned int i, len, variable_len;
         const struct ieee80211_mgmt *bcn;
         const u8 *ie;
         bool tim_found = 0;
+        unsigned int rate;
+        u16 ctl;
+        int antenna;
 
         bcn = (const struct ieee80211_mgmt *)(dev->wl->current_beacon->data);
         len = min((size_t) dev->wl->current_beacon->len,
                   0x200 - sizeof(struct b43_plcp_hdr6));
+        rate = dev->wl->beacon_txctl.tx_rate->hw_value;
 
         b43_write_template_common(dev, (const u8 *)bcn,
                                   len, ram_offset, shm_size_offset, rate);
 
+        /* Write the PHY TX control parameters. */
+        antenna = b43_antenna_from_ieee80211(dev,
+                        dev->wl->beacon_txctl.antenna_sel_tx);
+        antenna = b43_antenna_to_phyctl(antenna);
+        ctl = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_BEACPHYCTL);
+        /* We can't send beacons with short preamble. Would get PHY errors. */
+        ctl &= ~B43_TXH_PHY_SHORTPRMBL;
+        ctl &= ~B43_TXH_PHY_ANT;
+        ctl &= ~B43_TXH_PHY_ENC;
+        ctl |= antenna;
+        if (b43_is_cck_rate(rate))
+                ctl |= B43_TXH_PHY_ENC_CCK;
+        else
+                ctl |= B43_TXH_PHY_ENC_OFDM;
+        b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_BEACPHYCTL, ctl);
+
         /* Find the position of the TIM and the DTIM_period value
          * and write them to SHM. */
         ie = bcn->u.beacon.variable;
@@ -1474,8 +1559,7 @@ static void handle_irq_beacon(struct b43_wldev *dev)
 
         if (!beacon0_valid) {
                 if (!wl->beacon0_uploaded) {
-                        b43_write_beacon_template(dev, 0x68, 0x18,
-                                                  B43_CCK_RATE_1MB);
+                        b43_write_beacon_template(dev, 0x68, 0x18);
                         b43_write_probe_resp_template(dev, 0x268, 0x4A,
                                                       &__b43_ratetable[3]);
                         wl->beacon0_uploaded = 1;
@@ -1485,8 +1569,7 @@ static void handle_irq_beacon(struct b43_wldev *dev)
                 b43_write32(dev, B43_MMIO_MACCMD, cmd);
         } else if (!beacon1_valid) {
                 if (!wl->beacon1_uploaded) {
-                        b43_write_beacon_template(dev, 0x468, 0x1A,
-                                                  B43_CCK_RATE_1MB);
+                        b43_write_beacon_template(dev, 0x468, 0x1A);
                         wl->beacon1_uploaded = 1;
                 }
                 cmd = b43_read32(dev, B43_MMIO_MACCMD);
@@ -1519,7 +1602,8 @@ static void b43_beacon_update_trigger_work(struct work_struct *work)
 
 /* Asynchronously update the packet templates in template RAM.
  * Locking: Requires wl->irq_lock to be locked. */
-static void b43_update_templates(struct b43_wl *wl, struct sk_buff *beacon)
+static void b43_update_templates(struct b43_wl *wl, struct sk_buff *beacon,
+                                 const struct ieee80211_tx_control *txctl)
 {
         /* This is the top half of the ansynchronous beacon update.
          * The bottom half is the beacon IRQ.
@@ -1530,6 +1614,7 @@ static void b43_update_templates(struct b43_wl *wl, struct sk_buff *beacon)
         if (wl->current_beacon)
                 dev_kfree_skb_any(wl->current_beacon);
         wl->current_beacon = beacon;
+        memcpy(&wl->beacon_txctl, txctl, sizeof(wl->beacon_txctl));
         wl->beacon0_uploaded = 0;
         wl->beacon1_uploaded = 0;
         queue_work(wl->hw->workqueue, &wl->beacon_update_trigger);
@@ -2363,38 +2448,28 @@ static void b43_rate_memory_init(struct b43_wldev *dev)
         }
 }
 
+/* Set the default values for the PHY TX Control Words. */
+static void b43_set_phytxctl_defaults(struct b43_wldev *dev)
+{
+        u16 ctl = 0;
+
+        ctl |= B43_TXH_PHY_ENC_CCK;
+        ctl |= B43_TXH_PHY_ANT01AUTO;
+        ctl |= B43_TXH_PHY_TXPWR;
+
+        b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_BEACPHYCTL, ctl);
+        b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_ACKCTSPHYCTL, ctl);
+        b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_PRPHYCTL, ctl);
+}
+
 /* Set the TX-Antenna for management frames sent by firmware. */
 static void b43_mgmtframe_txantenna(struct b43_wldev *dev, int antenna)
 {
-        u16 ant = 0;
+        u16 ant;
         u16 tmp;
 
-        switch (antenna) {
-        case B43_ANTENNA0:
-                ant |= B43_TXH_PHY_ANT0;
-                break;
-        case B43_ANTENNA1:
-                ant |= B43_TXH_PHY_ANT1;
-                break;
-        case B43_ANTENNA2:
-                ant |= B43_TXH_PHY_ANT2;
-                break;
-        case B43_ANTENNA3:
-                ant |= B43_TXH_PHY_ANT3;
-                break;
-        case B43_ANTENNA_AUTO:
-                ant |= B43_TXH_PHY_ANT01AUTO;
-                break;
-        default:
-                B43_WARN_ON(1);
-        }
-
-        /* FIXME We also need to set the other flags of the PHY control field somewhere. */
+        ant = b43_antenna_to_phyctl(antenna);
 
-        /* For Beacons */
-        tmp = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_BEACPHYCTL);
-        tmp = (tmp & ~B43_TXH_PHY_ANT) | ant;
-        b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_BEACPHYCTL, tmp);
         /* For ACK/CTS */
         tmp = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_ACKCTSPHYCTL);
         tmp = (tmp & ~B43_TXH_PHY_ANT) | ant;
@@ -3112,52 +3187,6 @@ init_failure:
         return err;
 }
 
-/* Check if the use of the antenna that ieee80211 told us to
- * use is possible. This will fall back to DEFAULT.
- * "antenna_nr" is the antenna identifier we got from ieee80211. */
-u8 b43_ieee80211_antenna_sanitize(struct b43_wldev *dev,
-                                  u8 antenna_nr)
-{
-        u8 antenna_mask;
-
-        if (antenna_nr == 0) {
-                /* Zero means "use default antenna". That's always OK. */
-                return 0;
-        }
-
-        /* Get the mask of available antennas. */
-        if (dev->phy.gmode)
-                antenna_mask = dev->dev->bus->sprom.ant_available_bg;
-        else
-                antenna_mask = dev->dev->bus->sprom.ant_available_a;
-
-        if (!(antenna_mask & (1 << (antenna_nr - 1)))) {
-                /* This antenna is not available. Fall back to default. */
-                return 0;
-        }
-
-        return antenna_nr;
-}
-
-static int b43_antenna_from_ieee80211(struct b43_wldev *dev, u8 antenna)
-{
-        antenna = b43_ieee80211_antenna_sanitize(dev, antenna);
-        switch (antenna) {
-        case 0:         /* default/diversity */
-                return B43_ANTENNA_DEFAULT;
-        case 1:         /* Antenna 0 */
-                return B43_ANTENNA0;
-        case 2:         /* Antenna 1 */
-                return B43_ANTENNA1;
-        case 3:         /* Antenna 2 */
-                return B43_ANTENNA2;
-        case 4:         /* Antenna 3 */
-                return B43_ANTENNA3;
-        default:
-                return B43_ANTENNA_DEFAULT;
-        }
-}
-
 static int b43_op_config(struct ieee80211_hw *hw, struct ieee80211_conf *conf)
 {
         struct b43_wl *wl = hw_to_b43_wl(hw);
@@ -3405,8 +3434,10 @@ static int b43_op_config_interface(struct ieee80211_hw *hw,
                 if (b43_is_mode(wl, IEEE80211_IF_TYPE_AP)) {
                         B43_WARN_ON(conf->type != IEEE80211_IF_TYPE_AP);
                         b43_set_ssid(dev, conf->ssid, conf->ssid_len);
-                        if (conf->beacon)
-                                b43_update_templates(wl, conf->beacon);
+                        if (conf->beacon) {
+                                b43_update_templates(wl, conf->beacon,
+                                                     conf->beacon_control);
+                        }
                 }
                 b43_write_mac_bssid_templates(dev);
         }
@@ -3877,6 +3908,7 @@ static int b43_wireless_core_init(struct b43_wldev *dev)
         b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_PRMAXTIME, 1);
 
         b43_rate_memory_init(dev);
+        b43_set_phytxctl_defaults(dev);
 
         /* Minimum Contention Window */
         if (phy->type == B43_PHYTYPE_B) {
@@ -4087,16 +4119,17 @@ static int b43_op_beacon_set_tim(struct ieee80211_hw *hw, int aid, int set)
         struct b43_wl *wl = hw_to_b43_wl(hw);
         struct sk_buff *beacon;
         unsigned long flags;
+        struct ieee80211_tx_control txctl;
 
         /* 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);
+        beacon = ieee80211_beacon_get(hw, wl->vif, &txctl);
         if (unlikely(!beacon))
                 return -ENOMEM;
         spin_lock_irqsave(&wl->irq_lock, flags);
-        b43_update_templates(wl, beacon);
+        b43_update_templates(wl, beacon, &txctl);
         spin_unlock_irqrestore(&wl->irq_lock, flags);
 
         return 0;
@@ -4110,7 +4143,7 @@ static int b43_op_ibss_beacon_update(struct ieee80211_hw *hw,
         unsigned long flags;
 
         spin_lock_irqsave(&wl->irq_lock, flags);
-        b43_update_templates(wl, beacon);
+        b43_update_templates(wl, beacon, ctl);
         spin_unlock_irqrestore(&wl->irq_lock, flags);
 
         return 0;