b43: HT-PHY: implement RSSI polling

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

diff --git a/drivers/net/wireless/b43/phy_ht.c b/drivers/net/wireless/b43/phy_ht.c
index 22fdde613a58d7570fd543e986a660f6c067eb20..ae9cd2978060e9f8f16a8825fae684cee30a67ed 100644 (file)
--- a/drivers/net/wireless/b43/phy_ht.c
+++ b/drivers/net/wireless/b43/phy_ht.c
@@ -387,6 +387,92 @@ static void b43_phy_ht_tx_tone(struct b43_wldev *dev)
 }
 #endif
 
+/**************************************************
+ * RSSI
+ **************************************************/
+
+#if 0
+static void b43_phy_ht_rssi_select(struct b43_wldev *dev, u8 core_sel,
+                                  u8 rssi_type)
+{
+       static const u16 ctl_regs[3][2] = {
+               { B43_PHY_HT_AFE_C1, B43_PHY_HT_AFE_C1_OVER, },
+               { B43_PHY_HT_AFE_C2, B43_PHY_HT_AFE_C2_OVER, },
+               { B43_PHY_HT_AFE_C3, B43_PHY_HT_AFE_C3_OVER, },
+       };
+       static const u16 radio_r[] = { R2059_SYN, R2059_TXRX0, R2059_RXRX1, };
+       int core;
+
+       if (core_sel == 0) {
+               b43err(dev->wl, "RSSI selection for core off not implemented yet\n");
+       } else {
+               for (core = 0; core < 3; core++) {
+                       /* Check if caller requested a one specific core */
+                       if ((core_sel == 1 && core != 0) ||
+                           (core_sel == 2 && core != 1) ||
+                           (core_sel == 3 && core != 2))
+                               continue;
+
+                       switch (rssi_type) {
+                       case 4:
+                               b43_phy_set(dev, ctl_regs[core][0], 0x3 << 8);
+                               b43_phy_set(dev, ctl_regs[core][0], 0x3 << 10);
+                               b43_phy_set(dev, ctl_regs[core][1], 0x1 << 9);
+                               b43_phy_set(dev, ctl_regs[core][1], 0x1 << 10);
+
+                               b43_radio_set(dev, R2059_RXRX1 | 0xbf, 0x1);
+                               b43_radio_write(dev, radio_r[core] | 0x159,
+                                               0x11);
+                               break;
+                       default:
+                               b43err(dev->wl, "RSSI selection for type %d not implemented yet\n",
+                                      rssi_type);
+                       }
+               }
+       }
+}
+
+static void b43_phy_ht_poll_rssi(struct b43_wldev *dev, u8 type, s32 *buf,
+                                u8 nsamp)
+{
+       u16 phy_regs_values[12];
+       static const u16 phy_regs_to_save[] = {
+               B43_PHY_HT_AFE_C1, B43_PHY_HT_AFE_C1_OVER,
+               0x848, 0x841,
+               B43_PHY_HT_AFE_C2, B43_PHY_HT_AFE_C2_OVER,
+               0x868, 0x861,
+               B43_PHY_HT_AFE_C3, B43_PHY_HT_AFE_C3_OVER,
+               0x888, 0x881,
+       };
+       u16 tmp[3];
+       int i;
+
+       for (i = 0; i < 12; i++)
+               phy_regs_values[i] = b43_phy_read(dev, phy_regs_to_save[i]);
+
+       b43_phy_ht_rssi_select(dev, 5, type);
+
+       for (i = 0; i < 6; i++)
+               buf[i] = 0;
+
+       for (i = 0; i < nsamp; i++) {
+               tmp[0] = b43_phy_read(dev, B43_PHY_HT_RSSI_C1);
+               tmp[1] = b43_phy_read(dev, B43_PHY_HT_RSSI_C2);
+               tmp[2] = b43_phy_read(dev, B43_PHY_HT_RSSI_C3);
+
+               buf[0] += ((s8)((tmp[0] & 0x3F) << 2)) >> 2;
+               buf[1] += ((s8)(((tmp[0] >> 8) & 0x3F) << 2)) >> 2;
+               buf[2] += ((s8)((tmp[1] & 0x3F) << 2)) >> 2;
+               buf[3] += ((s8)(((tmp[1] >> 8) & 0x3F) << 2)) >> 2;
+               buf[4] += ((s8)((tmp[2] & 0x3F) << 2)) >> 2;
+               buf[5] += ((s8)(((tmp[2] >> 8) & 0x3F) << 2)) >> 2;
+       }
+
+       for (i = 0; i < 12; i++)
+               b43_phy_write(dev, phy_regs_to_save[i], phy_regs_values[i]);
+}
+#endif
+
 /**************************************************
  * Tx/Rx
  **************************************************/
@@ -454,12 +540,20 @@ 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;
+       s32 rssi_buf[6];
+
        /* TODO */
 
        b43_phy_ht_tx_tone(dev);
        udelay(20);
-       /* TODO: poll RSSI */
+       b43_phy_ht_poll_rssi(dev, 4, rssi_buf, 1);
        b43_phy_ht_stop_playback(dev);
+       b43_phy_ht_reset_cca(dev);
+
+       phy_ht->idle_tssi[0] = rssi_buf[0] & 0xff;
+       phy_ht->idle_tssi[1] = rssi_buf[2] & 0xff;
+       phy_ht->idle_tssi[2] = rssi_buf[4] & 0xff;
 
        /* TODO */
 }

diff --git a/drivers/net/wireless/b43/phy_ht.h b/drivers/net/wireless/b43/phy_ht.h
index c51795862b6d77a178276a49422b0ac0b6b13512..165c5014b7c8eb0b80e7d388f4b00fa63d8d6406 100644 (file)
--- a/drivers/net/wireless/b43/phy_ht.h
+++ b/drivers/net/wireless/b43/phy_ht.h
@@ -36,6 +36,9 @@
 #define  B43_PHY_HT_TXPCTL_CMD_C1_PCTLEN       0x8000  /* TX power control enable */
 #define B43_PHY_HT_TXPCTL_CMD_C2               0x222
 #define  B43_PHY_HT_TXPCTL_CMD_C2_INIT         0x007F
+#define B43_PHY_HT_RSSI_C1                     0x219
+#define B43_PHY_HT_RSSI_C2                     0x21A
+#define B43_PHY_HT_RSSI_C3                     0x21B
 
 #define B43_PHY_HT_C1_CLIP1THRES               B43_PHY_OFDM(0x00E)
 #define B43_PHY_HT_C2_CLIP1THRES               B43_PHY_OFDM(0x04E)
@@ -91,6 +94,8 @@ struct b43_phy_ht {
        u8 tx_pwr_idx[3];
 
        s32 bb_mult_save[3];
+
+       u8 idle_tssi[3];
 };