[PATCH 2/4] b43: N-PHY: add b43_nphy_pa_override

[PATCH 2/4] b43: N-PHY: add b43_nphy_pa_override

[Rafał Miłecki]

Signed-off-by: Rafał Miłecki <zajec5@gmail.com>
---
  drivers/net/wireless/b43/phy_n.c |   40 ++++++++++++++++++++++++++++++++++++-
  drivers/net/wireless/b43/phy_n.h |    3 ++
  2 files changed, 41 insertions(+), 2 deletions(-)

diff --git a/drivers/net/wireless/b43/phy_n.c b/drivers/net/wireless/b43/phy_n.c
index 5cd4b1a..739973e 100644
--- a/drivers/net/wireless/b43/phy_n.c
+++ b/drivers/net/wireless/b43/phy_n.c
@@ -330,6 +330,40 @@ void b43_nphy_bmac_clock_fgc(struct b43_wldev *dev, bool force)
  	ssb_write32(dev->dev, SSB_TMSLOW, tmslow);
  }

+/* http://bcm-v4.sipsolutions.net/802.11/PHY/N/PA%20override */
+static void b43_nphy_pa_override(struct b43_wldev *dev, bool enable)
+{
+	struct b43_phy_n *nphy = dev->phy.n;
+	enum ieee80211_band band;
+	u16 tmp;
+
+	if (!enable) {
+		nphy->rfctrl_intc1_save = b43_phy_read(dev,
+						       B43_NPHY_RFCTL_INTC1);
+		nphy->rfctrl_intc2_save = b43_phy_read(dev,
+						       B43_NPHY_RFCTL_INTC2);
+		band = b43_current_band(dev->wl);
+		if (dev->phy.rev >= 3) {
+			if (band == IEEE80211_BAND_5GHZ)
+				tmp = 0x600;
+			else
+				tmp = 0x480;
+		} else {
+			if (band == IEEE80211_BAND_5GHZ)
+				tmp = 0x180;
+			else
+				tmp = 0x120;
+		}
+		b43_phy_write(dev, B43_NPHY_RFCTL_INTC1, tmp);
+		b43_phy_write(dev, B43_NPHY_RFCTL_INTC2, tmp);
+	} else {
+		b43_phy_write(dev, B43_NPHY_RFCTL_INTC1,
+				nphy->rfctrl_intc1_save);
+		b43_phy_write(dev, B43_NPHY_RFCTL_INTC2,
+				nphy->rfctrl_intc2_save);
+	}
+}
+
  /* http://bcm-v4.sipsolutions.net/802.11/PHY/N/CCA */
  static void b43_nphy_reset_cca(struct b43_wldev *dev)
  {
@@ -662,10 +696,12 @@ int b43_phy_initn(struct b43_wldev *dev)
  	b43_nphy_bmac_clock_fgc(dev, 0);

  	//TODO N PHY MAC PHY Clock Set with argument 1
-	//TODO Disable PA Override
+
+	b43_nphy_pa_override(dev, false);
  	b43_nphy_force_rf_sequence(dev, B43_RFSEQ_RX2TX);
  	b43_nphy_force_rf_sequence(dev, B43_RFSEQ_RESET2RX);
-	//TODO Turn on PA override
+	b43_nphy_pa_override(dev, true);
+
  	//TODO N PHY Classifier with arguments 0 and 0
  	//TODO N PHY Det Clip with 0 and the clip array as arguments
  	tx_pwr_state = nphy->txpwrctrl;
diff --git a/drivers/net/wireless/b43/phy_n.h b/drivers/net/wireless/b43/phy_n.h
index f90e905..6bb9476 100644
--- a/drivers/net/wireless/b43/phy_n.h
+++ b/drivers/net/wireless/b43/phy_n.h
@@ -936,6 +936,9 @@ struct b43_phy_n {
  	bool hang_avoid;
  	bool mute;

+	u16 rfctrl_intc1_save;
+	u16 rfctrl_intc2_save;
+
  	u16 classifier_state;
  	u16 clip_state[2];

-- 
1.6.4.2