1 files changed, 55 insertions, 2 deletions

diff --git a/drivers/net/wireless/b43/phy_lp.c b/drivers/net/wireless/b43/phy_lp.c
index ed3a52886135..e68644d95024 100644
--- a/drivers/net/wireless/b43/phy_lp.c
+++ b/drivers/net/wireless/b43/phy_lp.c
@@ -346,9 +346,60 @@ static void lpphy_2063_init(struct b43_wldev *dev)
         b43_radio_write(dev, B2063_PA_SP2, 0x18);
 }
 
+struct lpphy_stx_table_entry {
+        u16 phy_offset;
+        u16 phy_shift;
+        u16 rf_addr;
+        u16 rf_shift;
+        u16 mask;
+};
+
+static const struct lpphy_stx_table_entry lpphy_stx_table[] = {
+        { .phy_offset = 2, .phy_shift = 6, .rf_addr = 0x3d, .rf_shift = 3, .mask = 0x01, },
+        { .phy_offset = 1, .phy_shift = 12, .rf_addr = 0x4c, .rf_shift = 1, .mask = 0x01, },
+        { .phy_offset = 1, .phy_shift = 8, .rf_addr = 0x50, .rf_shift = 0, .mask = 0x7f, },
+        { .phy_offset = 0, .phy_shift = 8, .rf_addr = 0x44, .rf_shift = 0, .mask = 0xff, },
+        { .phy_offset = 1, .phy_shift = 0, .rf_addr = 0x4a, .rf_shift = 0, .mask = 0xff, },
+        { .phy_offset = 0, .phy_shift = 4, .rf_addr = 0x4d, .rf_shift = 0, .mask = 0xff, },
+        { .phy_offset = 1, .phy_shift = 4, .rf_addr = 0x4e, .rf_shift = 0, .mask = 0xff, },
+        { .phy_offset = 0, .phy_shift = 12, .rf_addr = 0x4f, .rf_shift = 0, .mask = 0x0f, },
+        { .phy_offset = 1, .phy_shift = 0, .rf_addr = 0x4f, .rf_shift = 4, .mask = 0x0f, },
+        { .phy_offset = 3, .phy_shift = 0, .rf_addr = 0x49, .rf_shift = 0, .mask = 0x0f, },
+        { .phy_offset = 4, .phy_shift = 3, .rf_addr = 0x46, .rf_shift = 4, .mask = 0x07, },
+        { .phy_offset = 3, .phy_shift = 15, .rf_addr = 0x46, .rf_shift = 0, .mask = 0x01, },
+        { .phy_offset = 4, .phy_shift = 0, .rf_addr = 0x46, .rf_shift = 1, .mask = 0x07, },
+        { .phy_offset = 3, .phy_shift = 8, .rf_addr = 0x48, .rf_shift = 4, .mask = 0x07, },
+        { .phy_offset = 3, .phy_shift = 11, .rf_addr = 0x48, .rf_shift = 0, .mask = 0x0f, },
+        { .phy_offset = 3, .phy_shift = 4, .rf_addr = 0x49, .rf_shift = 4, .mask = 0x0f, },
+        { .phy_offset = 2, .phy_shift = 15, .rf_addr = 0x45, .rf_shift = 0, .mask = 0x01, },
+        { .phy_offset = 5, .phy_shift = 13, .rf_addr = 0x52, .rf_shift = 4, .mask = 0x07, },
+        { .phy_offset = 6, .phy_shift = 0, .rf_addr = 0x52, .rf_shift = 7, .mask = 0x01, },
+        { .phy_offset = 5, .phy_shift = 3, .rf_addr = 0x41, .rf_shift = 5, .mask = 0x07, },
+        { .phy_offset = 5, .phy_shift = 6, .rf_addr = 0x41, .rf_shift = 0, .mask = 0x0f, },
+        { .phy_offset = 5, .phy_shift = 10, .rf_addr = 0x42, .rf_shift = 5, .mask = 0x07, },
+        { .phy_offset = 4, .phy_shift = 15, .rf_addr = 0x42, .rf_shift = 0, .mask = 0x01, },
+        { .phy_offset = 5, .phy_shift = 0, .rf_addr = 0x42, .rf_shift = 1, .mask = 0x07, },
+        { .phy_offset = 4, .phy_shift = 11, .rf_addr = 0x43, .rf_shift = 4, .mask = 0x0f, },
+        { .phy_offset = 4, .phy_shift = 7, .rf_addr = 0x43, .rf_shift = 0, .mask = 0x0f, },
+        { .phy_offset = 4, .phy_shift = 6, .rf_addr = 0x45, .rf_shift = 1, .mask = 0x01, },
+        { .phy_offset = 2, .phy_shift = 7, .rf_addr = 0x40, .rf_shift = 4, .mask = 0x0f, },
+        { .phy_offset = 2, .phy_shift = 11, .rf_addr = 0x40, .rf_shift = 0, .mask = 0x0f, },
+};
+
 static void lpphy_sync_stx(struct b43_wldev *dev)
 {
-        //TODO
+        const struct lpphy_stx_table_entry *e;
+        unsigned int i;
+        u16 tmp;
+
+        for (i = 0; i < ARRAY_SIZE(lpphy_stx_table); i++) {
+                e = &lpphy_stx_table[i];
+                tmp = b43_radio_read(dev, e->rf_addr);
+                tmp >>= e->rf_shift;
+                tmp <<= e->phy_shift;
+                b43_phy_maskset(dev, B43_PHY_OFDM(0xF2 + e->phy_offset),
+                                e->mask << e->phy_shift, tmp);
+        }
 }
 
 static void lpphy_radio_init(struct b43_wldev *dev)
@@ -366,7 +417,9 @@ static void lpphy_radio_init(struct b43_wldev *dev)
                 lpphy_sync_stx(dev);
                 b43_phy_write(dev, B43_PHY_OFDM(0xF0), 0x5F80);
                 b43_phy_write(dev, B43_PHY_OFDM(0xF1), 0);
-                //TODO Do something on the backplane
+                if (dev->dev->bus->chip_id == 0x4325) {
+                        // TODO SSB PMU recalibration
+                }
         }
 }