1 files changed, 116 insertions, 83 deletions

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;