b43: N-PHY: add b43_nphy_pa_override

index 5cd4b1a..739973e 100644
@@ -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;

index f90e905..6bb9476 100644
@@ -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];