b43: N-PHY: add TX radio setup for newer PHYs

Signed-off-by: Rafał Miłecki <zajec5@gmail.com>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

1 files changed, 52 insertions, 1 deletions

diff --git a/drivers/net/wireless/b43/phy_n.c b/drivers/net/wireless/b43/phy_n.c
index 1dace55d243a..78e0cf6c78ca 100644
--- a/drivers/net/wireless/b43/phy_n.c
+++ b/drivers/net/wireless/b43/phy_n.c
@@ -1803,9 +1803,60 @@ static void b43_nphy_tx_cal_radio_setup(struct b43_wldev *dev)
 {
         struct b43_phy_n *nphy = dev->phy.n;
         u16 *save = nphy->tx_rx_cal_radio_saveregs;
+        u16 tmp;
+        u8 offset, i;
 
         if (dev->phy.rev >= 3) {
-                /* TODO */
+            for (i = 0; i < 2; i++) {
+                tmp = (i == 0) ? 0x2000 : 0x3000;
+                offset = i * 11;
+
+                save[offset + 0] = b43_radio_read16(dev, B2055_CAL_RVARCTL);
+                save[offset + 1] = b43_radio_read16(dev, B2055_CAL_LPOCTL);
+                save[offset + 2] = b43_radio_read16(dev, B2055_CAL_TS);
+                save[offset + 3] = b43_radio_read16(dev, B2055_CAL_RCCALRTS);
+                save[offset + 4] = b43_radio_read16(dev, B2055_CAL_RCALRTS);
+                save[offset + 5] = b43_radio_read16(dev, B2055_PADDRV);
+                save[offset + 6] = b43_radio_read16(dev, B2055_XOCTL1);
+                save[offset + 7] = b43_radio_read16(dev, B2055_XOCTL2);
+                save[offset + 8] = b43_radio_read16(dev, B2055_XOREGUL);
+                save[offset + 9] = b43_radio_read16(dev, B2055_XOMISC);
+                save[offset + 10] = b43_radio_read16(dev, B2055_PLL_LFC1);
+
+                if (b43_current_band(dev->wl) == IEEE80211_BAND_5GHZ) {
+                        b43_radio_write16(dev, tmp | B2055_CAL_RVARCTL, 0x0A);
+                        b43_radio_write16(dev, tmp | B2055_CAL_LPOCTL, 0x40);
+                        b43_radio_write16(dev, tmp | B2055_CAL_TS, 0x55);
+                        b43_radio_write16(dev, tmp | B2055_CAL_RCCALRTS, 0);
+                        b43_radio_write16(dev, tmp | B2055_CAL_RCALRTS, 0);
+                        if (nphy->ipa5g_on) {
+                                b43_radio_write16(dev, tmp | B2055_PADDRV, 4);
+                                b43_radio_write16(dev, tmp | B2055_XOCTL1, 1);
+                        } else {
+                                b43_radio_write16(dev, tmp | B2055_PADDRV, 0);
+                                b43_radio_write16(dev, tmp | B2055_XOCTL1, 0x2F);
+                        }
+                        b43_radio_write16(dev, tmp | B2055_XOCTL2, 0);
+                } else {
+                        b43_radio_write16(dev, tmp | B2055_CAL_RVARCTL, 0x06);
+                        b43_radio_write16(dev, tmp | B2055_CAL_LPOCTL, 0x40);
+                        b43_radio_write16(dev, tmp | B2055_CAL_TS, 0x55);
+                        b43_radio_write16(dev, tmp | B2055_CAL_RCCALRTS, 0);
+                        b43_radio_write16(dev, tmp | B2055_CAL_RCALRTS, 0);
+                        b43_radio_write16(dev, tmp | B2055_XOCTL1, 0);
+                        if (nphy->ipa2g_on) {
+                                b43_radio_write16(dev, tmp | B2055_PADDRV, 6);
+                                b43_radio_write16(dev, tmp | B2055_XOCTL2,
+                                        (dev->phy.rev < 5) ? 0x11 : 0x01);
+                        } else {
+                                b43_radio_write16(dev, tmp | B2055_PADDRV, 0);
+                                b43_radio_write16(dev, tmp | B2055_XOCTL2, 0);
+                        }
+                }
+                b43_radio_write16(dev, tmp | B2055_XOREGUL, 0);
+                b43_radio_write16(dev, tmp | B2055_XOMISC, 0);
+                b43_radio_write16(dev, tmp | B2055_PLL_LFC1, 0);
+            }
         } else {
                 save[0] = b43_radio_read16(dev, B2055_C1_TX_RF_IQCAL1);
                 b43_radio_write16(dev, B2055_C1_TX_RF_IQCAL1, 0x29);