b43: HT-PHY: finish calculating values for idle TSSI

[firefly-linux-kernel-4.4.55.git] / drivers / net / wireless / b43 / phy_ht.c

diff --git a/drivers/net/wireless/b43/phy_ht.c b/drivers/net/wireless/b43/phy_ht.c
index 3d0bddb64b3d1f6abc4367c52f82a883060a1247..fd9e249e95b8aa53a17d7b85069ad464a1f33392 100644 (file)
--- a/drivers/net/wireless/b43/phy_ht.c
+++ b/drivers/net/wireless/b43/phy_ht.c
@@ -529,9 +529,21 @@ static void b43_phy_ht_tx_power_ctl(struct b43_wldev *dev, bool enable)
 static void b43_phy_ht_tx_power_ctl_idle_tssi(struct b43_wldev *dev)
 {
        struct b43_phy_ht *phy_ht = dev->phy.ht;
+       static const u16 base[] = { 0x840, 0x860, 0x880 };
+       u16 save_regs[3][3];
        s32 rssi_buf[6];
+       int core;
 
-       /* TODO */
+       for (core = 0; core < 3; core++) {
+               save_regs[core][1] = b43_phy_read(dev, base[core] + 6);
+               save_regs[core][2] = b43_phy_read(dev, base[core] + 7);
+               save_regs[core][0] = b43_phy_read(dev, base[core] + 0);
+
+               b43_phy_write(dev, base[core] + 6, 0);
+               b43_phy_mask(dev, base[core] + 7, ~0xF); /* 0xF? Or just 0x6? */
+               b43_phy_set(dev, base[core] + 0, 0x0400);
+               b43_phy_set(dev, base[core] + 0, 0x1000);
+       }
 
        b43_phy_ht_tx_tone(dev);
        udelay(20);
@@ -543,7 +555,11 @@ static void b43_phy_ht_tx_power_ctl_idle_tssi(struct b43_wldev *dev)
        phy_ht->idle_tssi[1] = rssi_buf[2] & 0xff;
        phy_ht->idle_tssi[2] = rssi_buf[4] & 0xff;
 
-       /* TODO */
+       for (core = 0; core < 3; core++) {
+               b43_phy_write(dev, base[core] + 0, save_regs[core][0]);
+               b43_phy_write(dev, base[core] + 6, save_regs[core][1]);
+               b43_phy_write(dev, base[core] + 7, save_regs[core][2]);
+       }
 }
 
 static void b43_phy_ht_tx_power_ctl_setup(struct b43_wldev *dev)