[PATCH] bcm43xx: fix txpower reporting in WE.

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

4 files changed, 44 insertions, 15 deletions

diff --git a/drivers/net/wireless/bcm43xx/bcm43xx.h b/drivers/net/wireless/bcm43xx/bcm43xx.h
index 7b97d8bf79ed..981d563f5738 100644
--- a/drivers/net/wireless/bcm43xx/bcm43xx.h
+++ b/drivers/net/wireless/bcm43xx/bcm43xx.h
@@ -525,6 +525,8 @@ struct bcm43xx_radioinfo {
          * 3: tx_CTL2
          */
         u16 txpower[4];
+        /* Desired TX power in dBm Q5.2 */
+        u16 txpower_desired;
         /* Current Interference Mitigation mode */
         int interfmode;
         /* Stack of saved values from the Interference Mitigation code */
diff --git a/drivers/net/wireless/bcm43xx/bcm43xx_main.c b/drivers/net/wireless/bcm43xx/bcm43xx_main.c
index 1051a49ddafe..8e08c41f86de 100644
--- a/drivers/net/wireless/bcm43xx/bcm43xx_main.c
+++ b/drivers/net/wireless/bcm43xx/bcm43xx_main.c
@@ -793,6 +793,10 @@ static int bcm43xx_read_radioinfo(struct bcm43xx_private *bcm)
                 bcm->current_core->radio->txpower[2] = 3;
         else
                 bcm->current_core->radio->txpower[2] = 0;
+        if (bcm->current_core->phy->type == BCM43xx_PHYTYPE_A)
+                bcm->current_core->radio->txpower_desired = bcm->sprom.maxpower_aphy;
+        else
+                bcm->current_core->radio->txpower_desired = bcm->sprom.maxpower_bgphy;
 
         /* Initialize the in-memory nrssi Lookup Table. */
         for (i = 0; i < 64; i++)
diff --git a/drivers/net/wireless/bcm43xx/bcm43xx_phy.c b/drivers/net/wireless/bcm43xx/bcm43xx_phy.c
index d90f207b2473..d3c2fc1df375 100644
--- a/drivers/net/wireless/bcm43xx/bcm43xx_phy.c
+++ b/drivers/net/wireless/bcm43xx/bcm43xx_phy.c
@@ -1768,14 +1768,9 @@ void bcm43xx_phy_xmitpower(struct bcm43xx_private *bcm)
                         where REG is the max power as per the regulatory domain
                 */
 
-                /*TODO: Get desired_pwr from wx_handlers or the stack
-                limit_value(desired_pwr, 0, max_pwr);
-                */
-
-                desired_pwr = max_pwr; /* remove this when we have a real desired_pwr */
-
+                desired_pwr = limit_value(radio->txpower_desired, 0, max_pwr);
+                /* Check if we need to adjust the current power. */
                 pwr_adjust = desired_pwr - estimated_pwr;
-
                 radio_att_delta = -(pwr_adjust + 7) >> 3;
                 baseband_att_delta = -(pwr_adjust >> 1) - (4 * radio_att_delta);
                 if ((radio_att_delta == 0) && (baseband_att_delta == 0)) {
diff --git a/drivers/net/wireless/bcm43xx/bcm43xx_wx.c b/drivers/net/wireless/bcm43xx/bcm43xx_wx.c
index c1d788d50f49..bed7cfbe81d3 100644
--- a/drivers/net/wireless/bcm43xx/bcm43xx_wx.c
+++ b/drivers/net/wireless/bcm43xx/bcm43xx_wx.c
@@ -484,21 +484,40 @@ static int bcm43xx_wx_set_xmitpower(struct net_device *net_dev,
                                     char *extra)
 {
         struct bcm43xx_private *bcm = bcm43xx_priv(net_dev);
+        struct bcm43xx_radioinfo *radio;
+        struct bcm43xx_phyinfo *phy;
         unsigned long flags;
         int err = -ENODEV;
+        u16 maxpower;
 
         wx_enter();
 
+        if ((data->txpower.flags & IW_TXPOW_TYPE) != IW_TXPOW_DBM) {
+                printk(PFX KERN_ERR "TX power not in dBm.\n");
+                return -EOPNOTSUPP;
+        }
+
         spin_lock_irqsave(&bcm->lock, flags);
         if (!bcm->initialized)
                 goto out_unlock;
-        if (data->power.disabled != (!(bcm->current_core->radio->enabled))) {
-                if (data->power.disabled)
+        radio = bcm->current_core->radio;
+        phy = bcm->current_core->phy;
+        if (data->txpower.disabled != (!(radio->enabled))) {
+                if (data->txpower.disabled)
                         bcm43xx_radio_turn_off(bcm);
                 else
                         bcm43xx_radio_turn_on(bcm);
         }
-        //TODO: set txpower.
+        if (data->txpower.value > 0) {
+                /* desired and maxpower dBm values are in Q5.2 */
+                if (phy->type == BCM43xx_PHYTYPE_A)
+                        maxpower = bcm->sprom.maxpower_aphy;
+                else
+                        maxpower = bcm->sprom.maxpower_bgphy;
+                radio->txpower_desired = limit_value(data->txpower.value << 2,
+                                                     0, maxpower);
+                bcm43xx_phy_xmitpower(bcm);
+        }
         err = 0;
 
 out_unlock:
@@ -513,18 +532,27 @@ static int bcm43xx_wx_get_xmitpower(struct net_device *net_dev,
                                     char *extra)
 {
         struct bcm43xx_private *bcm = bcm43xx_priv(net_dev);
+        struct bcm43xx_radioinfo *radio;
         unsigned long flags;
+        int err = -ENODEV;
 
         wx_enter();
 
         spin_lock_irqsave(&bcm->lock, flags);
-//TODO  data->power.value = ???
-        data->power.fixed = 1;
-        data->power.flags = IW_TXPOW_DBM;
-        data->power.disabled = !(bcm->current_core->radio->enabled);
+        if (!bcm->initialized)
+                goto out_unlock;
+        radio = bcm->current_core->radio;
+        /* desired dBm value is in Q5.2 */
+        data->txpower.value = radio->txpower_desired >> 2;
+        data->txpower.fixed = 1;
+        data->txpower.flags = IW_TXPOW_DBM;
+        data->txpower.disabled = !(radio->enabled);
+
+        err = 0;
+out_unlock:
         spin_unlock_irqrestore(&bcm->lock, flags);
 
-        return 0;
+        return err;
 }
 
 static int bcm43xx_wx_set_retry(struct net_device *net_dev,