| 1 | --- a/drivers/net/wireless/b43/main.c |
| 2 | +++ b/drivers/net/wireless/b43/main.c |
| 3 | @@ -1529,7 +1529,7 @@ static void b43_write_beacon_template(st |
| 4 | len, ram_offset, shm_size_offset, rate); |
| 5 | |
| 6 | /* Write the PHY TX control parameters. */ |
| 7 | - antenna = B43_ANTENNA_DEFAULT; |
| 8 | + antenna = dev->tx_antenna; |
| 9 | antenna = b43_antenna_to_phyctl(antenna); |
| 10 | ctl = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_BEACPHYCTL); |
| 11 | /* We can't send beacons with short preamble. Would get PHY errors. */ |
| 12 | @@ -3049,8 +3049,8 @@ static int b43_chip_init(struct b43_wlde |
| 13 | |
| 14 | /* Select the antennae */ |
| 15 | if (phy->ops->set_rx_antenna) |
| 16 | - phy->ops->set_rx_antenna(dev, B43_ANTENNA_DEFAULT); |
| 17 | - b43_mgmtframe_txantenna(dev, B43_ANTENNA_DEFAULT); |
| 18 | + phy->ops->set_rx_antenna(dev, dev->rx_antenna); |
| 19 | + b43_mgmtframe_txantenna(dev, dev->tx_antenna); |
| 20 | |
| 21 | if (phy->type == B43_PHYTYPE_B) { |
| 22 | value16 = b43_read16(dev, 0x005E); |
| 23 | @@ -3794,7 +3794,6 @@ static int b43_op_config(struct ieee8021 |
| 24 | struct b43_wldev *dev; |
| 25 | struct b43_phy *phy; |
| 26 | struct ieee80211_conf *conf = &hw->conf; |
| 27 | - int antenna; |
| 28 | int err = 0; |
| 29 | bool reload_bss = false; |
| 30 | |
| 31 | @@ -3848,11 +3847,9 @@ static int b43_op_config(struct ieee8021 |
| 32 | } |
| 33 | |
| 34 | /* Antennas for RX and management frame TX. */ |
| 35 | - antenna = B43_ANTENNA_DEFAULT; |
| 36 | - b43_mgmtframe_txantenna(dev, antenna); |
| 37 | - antenna = B43_ANTENNA_DEFAULT; |
| 38 | + b43_mgmtframe_txantenna(dev, dev->tx_antenna); |
| 39 | if (phy->ops->set_rx_antenna) |
| 40 | - phy->ops->set_rx_antenna(dev, antenna); |
| 41 | + phy->ops->set_rx_antenna(dev, dev->rx_antenna); |
| 42 | |
| 43 | if (wl->radio_enabled != phy->radio_on) { |
| 44 | if (wl->radio_enabled) { |
| 45 | @@ -4974,6 +4971,47 @@ static int b43_op_get_survey(struct ieee |
| 46 | return 0; |
| 47 | } |
| 48 | |
| 49 | +static int b43_op_set_antenna(struct ieee80211_hw *hw, u32 tx_ant, u32 rx_ant) |
| 50 | +{ |
| 51 | + struct b43_wl *wl = hw_to_b43_wl(hw); |
| 52 | + struct b43_wldev *dev = wl->current_dev; |
| 53 | + |
| 54 | + if (tx_ant == 1 && rx_ant == 1) { |
| 55 | + dev->tx_antenna = B43_ANTENNA0; |
| 56 | + dev->rx_antenna = B43_ANTENNA0; |
| 57 | + } |
| 58 | + else if (tx_ant == 2 && rx_ant == 2) { |
| 59 | + dev->tx_antenna = B43_ANTENNA1; |
| 60 | + dev->rx_antenna = B43_ANTENNA1; |
| 61 | + } |
| 62 | + else if ((tx_ant & 3) == 3 && (rx_ant & 3) == 3) { |
| 63 | + dev->tx_antenna = B43_ANTENNA_DEFAULT; |
| 64 | + dev->rx_antenna = B43_ANTENNA_DEFAULT; |
| 65 | + } |
| 66 | + else { |
| 67 | + return -EINVAL; |
| 68 | + } |
| 69 | + |
| 70 | + return 0; |
| 71 | +} |
| 72 | + |
| 73 | + |
| 74 | +static int b43_op_get_antenna(struct ieee80211_hw *hw, u32 *tx_ant, u32 *rx_ant) |
| 75 | +{ |
| 76 | + struct b43_wl *wl = hw_to_b43_wl(hw); |
| 77 | + struct b43_wldev *dev = wl->current_dev; |
| 78 | + |
| 79 | + switch (dev->tx_antenna) { |
| 80 | + case B43_ANTENNA0: |
| 81 | + *tx_ant = 1; *rx_ant = 1; break; |
| 82 | + case B43_ANTENNA1: |
| 83 | + *tx_ant = 2; *rx_ant = 2; break; |
| 84 | + case B43_ANTENNA_DEFAULT: |
| 85 | + *tx_ant = 3; *rx_ant = 3; break; |
| 86 | + } |
| 87 | + return 0; |
| 88 | +} |
| 89 | + |
| 90 | static const struct ieee80211_ops b43_hw_ops = { |
| 91 | .tx = b43_op_tx, |
| 92 | .conf_tx = b43_op_conf_tx, |
| 93 | @@ -4995,6 +5033,8 @@ static const struct ieee80211_ops b43_hw |
| 94 | .sw_scan_complete = b43_op_sw_scan_complete_notifier, |
| 95 | .get_survey = b43_op_get_survey, |
| 96 | .rfkill_poll = b43_rfkill_poll, |
| 97 | + .set_antenna = b43_op_set_antenna, |
| 98 | + .get_antenna = b43_op_get_antenna, |
| 99 | }; |
| 100 | |
| 101 | /* Hard-reset the chip. Do not call this directly. |
| 102 | @@ -5241,6 +5281,8 @@ static int b43_one_core_attach(struct b4 |
| 103 | if (!wldev) |
| 104 | goto out; |
| 105 | |
| 106 | + wldev->rx_antenna = B43_ANTENNA_DEFAULT; |
| 107 | + wldev->tx_antenna = B43_ANTENNA_DEFAULT; |
| 108 | wldev->use_pio = b43_modparam_pio; |
| 109 | wldev->dev = dev; |
| 110 | wldev->wl = wl; |
| 111 | @@ -5331,6 +5373,9 @@ static struct b43_wl *b43_wireless_init( |
| 112 | |
| 113 | hw->wiphy->flags |= WIPHY_FLAG_IBSS_RSN; |
| 114 | |
| 115 | + hw->wiphy->available_antennas_rx = 0x3; |
| 116 | + hw->wiphy->available_antennas_tx = 0x3; |
| 117 | + |
| 118 | wl->hw_registred = false; |
| 119 | hw->max_rates = 2; |
| 120 | SET_IEEE80211_DEV(hw, dev->dev); |
| 121 | --- a/drivers/net/wireless/b43/b43.h |
| 122 | +++ b/drivers/net/wireless/b43/b43.h |
| 123 | @@ -808,6 +808,8 @@ struct b43_wldev { |
| 124 | bool hwcrypto_enabled; /* TRUE, if HW crypto acceleration is enabled. */ |
| 125 | bool use_pio; /* TRUE if next init should use PIO */ |
| 126 | int gpiomask; /* GPIO LED mask as a module parameter */ |
| 127 | + int rx_antenna; /* Used RX antenna (B43_ANTENNAxxx) */ |
| 128 | + int tx_antenna; /* Used TX antenna (B43_ANTENNAxxx) */ |
| 129 | |
| 130 | /* PHY/Radio device. */ |
| 131 | struct b43_phy phy; |
| 132 | |
