b43: Add parts of LP-PHY TX power control

This adds the initial parts of the LP-PHY TX power control.
This also adds helper functions for bulk access of LP tables.

Signed-off-by: Michael Buesch <mb@bu3sch.de>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

1 files changed, 175 insertions, 2 deletions

diff --git a/drivers/net/wireless/b43/phy_lp.c b/drivers/net/wireless/b43/phy_lp.c
index 94d9e8b215e3..58e319d6b1ed 100644
--- a/drivers/net/wireless/b43/phy_lp.c
+++ b/drivers/net/wireless/b43/phy_lp.c
@@ -23,6 +23,7 @@
 */
 
 #include "b43.h"
+#include "main.h"
 #include "phy_lp.h"
 #include "phy_common.h"
 #include "tables_lpphy.h"
@@ -267,13 +268,185 @@ static void lpphy_radio_init(struct b43_wldev *dev)
         }
 }
 
+/* Read the TX power control mode from hardware. */
+static void lpphy_read_tx_pctl_mode_from_hardware(struct b43_wldev *dev)
+{
+        struct b43_phy_lp *lpphy = dev->phy.lp;
+        u16 ctl;
+
+        ctl = b43_phy_read(dev, B43_LPPHY_TX_PWR_CTL_CMD);
+        switch (ctl & B43_LPPHY_TX_PWR_CTL_CMD_MODE) {
+        case B43_LPPHY_TX_PWR_CTL_CMD_MODE_OFF:
+                lpphy->txpctl_mode = B43_LPPHY_TXPCTL_OFF;
+                break;
+        case B43_LPPHY_TX_PWR_CTL_CMD_MODE_SW:
+                lpphy->txpctl_mode = B43_LPPHY_TXPCTL_SW;
+                break;
+        case B43_LPPHY_TX_PWR_CTL_CMD_MODE_HW:
+                lpphy->txpctl_mode = B43_LPPHY_TXPCTL_HW;
+                break;
+        default:
+                lpphy->txpctl_mode = B43_LPPHY_TXPCTL_UNKNOWN;
+                B43_WARN_ON(1);
+                break;
+        }
+}
+
+/* Set the TX power control mode in hardware. */
+static void lpphy_write_tx_pctl_mode_to_hardware(struct b43_wldev *dev)
+{
+        struct b43_phy_lp *lpphy = dev->phy.lp;
+        u16 ctl;
+
+        switch (lpphy->txpctl_mode) {
+        case B43_LPPHY_TXPCTL_OFF:
+                ctl = B43_LPPHY_TX_PWR_CTL_CMD_MODE_OFF;
+                break;
+        case B43_LPPHY_TXPCTL_HW:
+                ctl = B43_LPPHY_TX_PWR_CTL_CMD_MODE_HW;
+                break;
+        case B43_LPPHY_TXPCTL_SW:
+                ctl = B43_LPPHY_TX_PWR_CTL_CMD_MODE_SW;
+                break;
+        default:
+                ctl = 0;
+                B43_WARN_ON(1);
+        }
+        b43_phy_maskset(dev, B43_LPPHY_TX_PWR_CTL_CMD,
+                        (u16)~B43_LPPHY_TX_PWR_CTL_CMD_MODE, ctl);
+}
+
+static void lpphy_set_tx_power_control(struct b43_wldev *dev,
+                                       enum b43_lpphy_txpctl_mode mode)
+{
+        struct b43_phy_lp *lpphy = dev->phy.lp;
+        enum b43_lpphy_txpctl_mode oldmode;
+
+        oldmode = lpphy->txpctl_mode;
+        lpphy_read_tx_pctl_mode_from_hardware(dev);
+        if (lpphy->txpctl_mode == mode)
+                return;
+        lpphy->txpctl_mode = mode;
+
+        if (oldmode == B43_LPPHY_TXPCTL_HW) {
+                //TODO Update TX Power NPT
+                //TODO Clear all TX Power offsets
+        } else {
+                if (mode == B43_LPPHY_TXPCTL_HW) {
+                        //TODO Recalculate target TX power
+                        b43_phy_maskset(dev, B43_LPPHY_TX_PWR_CTL_CMD,
+                                        0xFF80, lpphy->tssi_idx);
+                        b43_phy_maskset(dev, B43_LPPHY_TX_PWR_CTL_NNUM,
+                                        0x8FFF, ((u16)lpphy->tssi_npt << 16));
+                        //TODO Set "TSSI Transmit Count" variable to total transmitted frame count
+                        //TODO Disable TX gain override
+                        lpphy->tx_pwr_idx_over = -1;
+                }
+        }
+        if (dev->phy.rev >= 2) {
+                if (mode == B43_LPPHY_TXPCTL_HW)
+                        b43_phy_maskset(dev, B43_PHY_OFDM(0xD0), 0xFD, 0x2);
+                else
+                        b43_phy_maskset(dev, B43_PHY_OFDM(0xD0), 0xFD, 0);
+        }
+        lpphy_write_tx_pctl_mode_to_hardware(dev);
+}
+
+static void lpphy_set_tx_power_by_index(struct b43_wldev *dev, u8 index)
+{
+        struct b43_phy_lp *lpphy = dev->phy.lp;
+
+        lpphy->tx_pwr_idx_over = index;
+        if (lpphy->txpctl_mode != B43_LPPHY_TXPCTL_OFF)
+                lpphy_set_tx_power_control(dev, B43_LPPHY_TXPCTL_SW);
+
+        //TODO
+}
+
+static void lpphy_btcoex_override(struct b43_wldev *dev)
+{
+        b43_write16(dev, B43_MMIO_BTCOEX_CTL, 0x3);
+        b43_write16(dev, B43_MMIO_BTCOEX_TXCTL, 0xFF);
+}
+
+static void lpphy_pr41573_workaround(struct b43_wldev *dev)
+{
+        struct b43_phy_lp *lpphy = dev->phy.lp;
+        u32 *saved_tab;
+        const unsigned int saved_tab_size = 256;
+        enum b43_lpphy_txpctl_mode txpctl_mode;
+        s8 tx_pwr_idx_over;
+        u16 tssi_npt, tssi_idx;
+
+        saved_tab = kcalloc(saved_tab_size, sizeof(saved_tab[0]), GFP_KERNEL);
+        if (!saved_tab) {
+                b43err(dev->wl, "PR41573 failed. Out of memory!\n");
+                return;
+        }
+
+        lpphy_read_tx_pctl_mode_from_hardware(dev);
+        txpctl_mode = lpphy->txpctl_mode;
+        tx_pwr_idx_over = lpphy->tx_pwr_idx_over;
+        tssi_npt = lpphy->tssi_npt;
+        tssi_idx = lpphy->tssi_idx;
+
+        if (dev->phy.rev < 2) {
+                b43_lptab_read_bulk(dev, B43_LPTAB32(10, 0x140),
+                                    saved_tab_size, saved_tab);
+        } else {
+                b43_lptab_read_bulk(dev, B43_LPTAB32(7, 0x140),
+                                    saved_tab_size, saved_tab);
+        }
+        //TODO
+
+        kfree(saved_tab);
+}
+
+static void lpphy_calibration(struct b43_wldev *dev)
+{
+        struct b43_phy_lp *lpphy = dev->phy.lp;
+        enum b43_lpphy_txpctl_mode saved_pctl_mode;
+
+        b43_mac_suspend(dev);
+
+        lpphy_btcoex_override(dev);
+        lpphy_read_tx_pctl_mode_from_hardware(dev);
+        saved_pctl_mode = lpphy->txpctl_mode;
+        lpphy_set_tx_power_control(dev, B43_LPPHY_TXPCTL_OFF);
+        //TODO Perform transmit power table I/Q LO calibration
+        if ((dev->phy.rev == 0) && (saved_pctl_mode != B43_LPPHY_TXPCTL_OFF))
+                lpphy_pr41573_workaround(dev);
+        //TODO If a full calibration has not been performed on this channel yet, perform PAPD TX-power calibration
+        lpphy_set_tx_power_control(dev, saved_pctl_mode);
+        //TODO Perform I/Q calibration with a single control value set
+
+        b43_mac_enable(dev);
+}
+
+/* Initialize TX power control */
+static void lpphy_tx_pctl_init(struct b43_wldev *dev)
+{
+        if (0/*FIXME HWPCTL capable */) {
+                //TODO
+        } else { /* This device is only software TX power control capable. */
+                if (b43_current_band(dev->wl) == IEEE80211_BAND_2GHZ) {
+                        //TODO
+                } else {
+                        //TODO
+                }
+                //TODO set BB multiplier to 0x0096
+        }
+}
+
 static int b43_lpphy_op_init(struct b43_wldev *dev)
 {
         /* TODO: band SPROM */
         lpphy_baseband_init(dev);
         lpphy_radio_init(dev);
-
-        //TODO
+        //TODO calibrate RC
+        //TODO set channel
+        lpphy_tx_pctl_init(dev);
+        //TODO full calib
 
         return 0;
 }