9 files changed, 341 insertions, 121 deletions

diff --git a/drivers/net/wireless/b43/b43.h b/drivers/net/wireless/b43/b43.h
index fc280157596c..f9c8161671d9 100644
--- a/drivers/net/wireless/b43/b43.h
+++ b/drivers/net/wireless/b43/b43.h
@@ -173,6 +173,11 @@ enum {
 #define B43_SHM_SH_CHAN                 0x00A0  /* Current channel (low 8bit only) */
 #define  B43_SHM_SH_CHAN_5GHZ           0x0100  /* Bit set, if 5Ghz channel */
 #define B43_SHM_SH_BCMCFIFOID           0x0108  /* Last posted cookie to the bcast/mcast FIFO */
+/* TSSI information */
+#define B43_SHM_SH_TSSI_CCK             0x0058  /* TSSI for last 4 CCK frames (32bit) */
+#define B43_SHM_SH_TSSI_OFDM_A          0x0068  /* TSSI for last 4 OFDM frames (32bit) */
+#define B43_SHM_SH_TSSI_OFDM_G          0x0070  /* TSSI for last 4 OFDM frames (32bit) */
+#define  B43_TSSI_MAX                   0x7F    /* Max value for one TSSI value */
 /* SHM_SHARED TX FIFO variables */
 #define B43_SHM_SH_SIZE01               0x0098  /* TX FIFO size for FIFO 0 (low) and 1 (high) */
 #define B43_SHM_SH_SIZE23               0x009A  /* TX FIFO size for FIFO 2 and 3 */
@@ -648,6 +653,11 @@ struct b43_wl {
         struct b43_qos_params qos_params[4];
         /* Workqueue for updating QOS parameters in hardware. */
         struct work_struct qos_update_work;
+
+        /* Work for adjustment of the transmission power.
+         * This is scheduled when we determine that the actual TX output
+         * power doesn't match what we want. */
+        struct work_struct txpower_adjust_work;
 };
 
 /* In-memory representation of a cached microcode file. */
diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/b43/main.c
index af43f03b3189..63bafc2f3f0a 100644
--- a/drivers/net/wireless/b43/main.c
+++ b/drivers/net/wireless/b43/main.c
@@ -2805,6 +2805,9 @@ static void b43_periodic_every60sec(struct b43_wldev *dev)
 
         if (ops->pwork_60sec)
                 ops->pwork_60sec(dev);
+
+        /* Force check the TX power emission now. */
+        b43_phy_txpower_check(dev, B43_TXPWR_IGNORE_TIME);
 }
 
 static void b43_periodic_every30sec(struct b43_wldev *dev)
@@ -2835,8 +2838,6 @@ static void b43_periodic_every15sec(struct b43_wldev *dev)
         if (phy->ops->pwork_15sec)
                 phy->ops->pwork_15sec(dev);
 
-        phy->ops->xmitpower(dev);
-
         atomic_set(&phy->txerr_cnt, B43_PHY_TX_BADNESS_LIMIT);
         wmb();
 }
@@ -3382,10 +3383,13 @@ static int b43_op_config(struct ieee80211_hw *hw, struct ieee80211_conf *conf)
 
         /* Adjust the desired TX power level. */
         if (conf->power_level != 0) {
-                if (conf->power_level != phy->power_level) {
-                        phy->power_level = conf->power_level;
-                        phy->ops->xmitpower(dev);
+                spin_lock_irqsave(&wl->irq_lock, flags);
+                if (conf->power_level != phy->desired_txpower) {
+                        phy->desired_txpower = conf->power_level;
+                        b43_phy_txpower_check(dev, B43_TXPWR_IGNORE_TIME |
+                                                   B43_TXPWR_IGNORE_TSSI);
                 }
+                spin_unlock_irqrestore(&wl->irq_lock, flags);
         }
 
         /* Antennas for RX and management frame TX. */
@@ -3785,6 +3789,7 @@ static void setup_struct_phy_for_init(struct b43_wldev *dev,
                                       struct b43_phy *phy)
 {
         phy->hardware_power_control = !!modparam_hwpctl;
+        phy->next_txpwr_check_time = jiffies;
         /* PHY TX errors counter. */
         atomic_set(&phy->txerr_cnt, B43_PHY_TX_BADNESS_LIMIT);
 }
@@ -4204,6 +4209,8 @@ static void b43_op_stop(struct ieee80211_hw *hw)
                 b43_wireless_core_stop(dev);
         b43_wireless_core_exit(dev);
         mutex_unlock(&wl->mutex);
+
+        cancel_work_sync(&(wl->txpower_adjust_work));
 }
 
 static int b43_op_set_retry_limit(struct ieee80211_hw *hw,
@@ -4581,6 +4588,7 @@ static int b43_wireless_init(struct ssb_device *dev)
         INIT_LIST_HEAD(&wl->devlist);
         INIT_WORK(&wl->qos_update_work, b43_qos_update_work);
         INIT_WORK(&wl->beacon_update_trigger, b43_beacon_update_trigger_work);
+        INIT_WORK(&wl->txpower_adjust_work, b43_phy_txpower_adjust_work);
 
         ssb_set_devtypedata(dev, wl);
         b43info(wl, "Broadcom %04X WLAN found\n", dev->bus->chip_id);
diff --git a/drivers/net/wireless/b43/nphy.c b/drivers/net/wireless/b43/nphy.c
index 831986c459f8..4cfeab8560f6 100644
--- a/drivers/net/wireless/b43/nphy.c
+++ b/drivers/net/wireless/b43/nphy.c
@@ -34,10 +34,16 @@ void b43_nphy_set_rxantenna(struct b43_wldev *dev, int antenna)
 {//TODO
 }
 
-void b43_nphy_xmitpower(struct b43_wldev *dev)
+static void b43_nphy_op_adjust_txpower(struct b43_wldev *dev)
 {//TODO
 }
 
+static enum b43_txpwr_result b43_nphy_op_recalc_txpower(struct b43_wldev *dev,
+                                                        bool ignore_tssi)
+{//TODO
+        return B43_TXPWR_RES_DONE;
+}
+
 static void b43_chantab_radio_upload(struct b43_wldev *dev,
                                      const struct b43_nphy_channeltab_entry *e)
 {
@@ -602,10 +608,6 @@ static unsigned int b43_nphy_op_get_default_chan(struct b43_wldev *dev)
         return 36;
 }
 
-static void b43_nphy_op_xmitpower(struct b43_wldev *dev)
-{//TODO
-}
-
 const struct b43_phy_operations b43_phyops_n = {
         .allocate               = b43_nphy_op_allocate,
         .init                   = b43_nphy_op_init,
@@ -617,5 +619,6 @@ const struct b43_phy_operations b43_phyops_n = {
         .software_rfkill        = b43_nphy_op_software_rfkill,
         .switch_channel         = b43_nphy_op_switch_channel,
         .get_default_chan       = b43_nphy_op_get_default_chan,
-        .xmitpower              = b43_nphy_op_xmitpower,
+        .recalc_txpower         = b43_nphy_op_recalc_txpower,
+        .adjust_txpower         = b43_nphy_op_adjust_txpower,
 };
diff --git a/drivers/net/wireless/b43/phy_a.c b/drivers/net/wireless/b43/phy_a.c
index dd347314b76c..4d7d59e30960 100644
--- a/drivers/net/wireless/b43/phy_a.c
+++ b/drivers/net/wireless/b43/phy_a.c
@@ -505,10 +505,16 @@ static void b43_aphy_op_set_rx_antenna(struct b43_wldev *dev, int antenna)
         b43_hf_write(dev, hf);
 }
 
-static void b43_aphy_op_xmitpower(struct b43_wldev *dev)
+static void b43_aphy_op_adjust_txpower(struct b43_wldev *dev)
 {//TODO
 }
 
+static enum b43_txpwr_result b43_aphy_op_recalc_txpower(struct b43_wldev *dev,
+                                                        bool ignore_tssi)
+{//TODO
+        return B43_TXPWR_RES_DONE;
+}
+
 static void b43_aphy_op_pwork_15sec(struct b43_wldev *dev)
 {//TODO
 }
@@ -530,7 +536,8 @@ const struct b43_phy_operations b43_phyops_a = {
         .switch_channel         = b43_aphy_op_switch_channel,
         .get_default_chan       = b43_aphy_op_get_default_chan,
         .set_rx_antenna         = b43_aphy_op_set_rx_antenna,
-        .xmitpower              = b43_aphy_op_xmitpower,
+        .recalc_txpower         = b43_aphy_op_recalc_txpower,
+        .adjust_txpower         = b43_aphy_op_adjust_txpower,
         .pwork_15sec            = b43_aphy_op_pwork_15sec,
         .pwork_60sec            = b43_aphy_op_pwork_60sec,
 };
diff --git a/drivers/net/wireless/b43/phy_common.c b/drivers/net/wireless/b43/phy_common.c
index 45074c05d51d..5a550a7af2e9 100644
--- a/drivers/net/wireless/b43/phy_common.c
+++ b/drivers/net/wireless/b43/phy_common.c
@@ -274,3 +274,94 @@ void b43_software_rfkill(struct b43_wldev *dev, enum rfkill_state state)
         phy->ops->software_rfkill(dev, state);
         phy->radio_on = (state == RFKILL_STATE_UNBLOCKED);
 }
+
+/**
+ * b43_phy_txpower_adjust_work - TX power workqueue.
+ *
+ * Workqueue for updating the TX power parameters in hardware.
+ */
+void b43_phy_txpower_adjust_work(struct work_struct *work)
+{
+        struct b43_wl *wl = container_of(work, struct b43_wl,
+                                         txpower_adjust_work);
+        struct b43_wldev *dev;
+
+        mutex_lock(&wl->mutex);
+        dev = wl->current_dev;
+
+        if (likely(dev && (b43_status(dev) >= B43_STAT_STARTED)))
+                dev->phy.ops->adjust_txpower(dev);
+
+        mutex_unlock(&wl->mutex);
+}
+
+/* Called with wl->irq_lock locked */
+void b43_phy_txpower_check(struct b43_wldev *dev, unsigned int flags)
+{
+        struct b43_phy *phy = &dev->phy;
+        unsigned long now = jiffies;
+        enum b43_txpwr_result result;
+
+        if (!(flags & B43_TXPWR_IGNORE_TIME)) {
+                /* Check if it's time for a TXpower check. */
+                if (time_before(now, phy->next_txpwr_check_time))
+                        return; /* Not yet */
+        }
+        /* The next check will be needed in two seconds, or later. */
+        phy->next_txpwr_check_time = round_jiffies(now + (HZ * 2));
+
+        if ((dev->dev->bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM) &&
+            (dev->dev->bus->boardinfo.type == SSB_BOARD_BU4306))
+                return; /* No software txpower adjustment needed */
+
+        result = phy->ops->recalc_txpower(dev, !!(flags & B43_TXPWR_IGNORE_TSSI));
+        if (result == B43_TXPWR_RES_DONE)
+                return; /* We are done. */
+        B43_WARN_ON(result != B43_TXPWR_RES_NEED_ADJUST);
+        B43_WARN_ON(phy->ops->adjust_txpower == NULL);
+
+        /* We must adjust the transmission power in hardware.
+         * Schedule b43_phy_txpower_adjust_work(). */
+        queue_work(dev->wl->hw->workqueue, &dev->wl->txpower_adjust_work);
+}
+
+int b43_phy_shm_tssi_read(struct b43_wldev *dev, u16 shm_offset)
+{
+        const bool is_ofdm = (shm_offset != B43_SHM_SH_TSSI_CCK);
+        unsigned int a, b, c, d;
+        unsigned int average;
+        u32 tmp;
+
+        tmp = b43_shm_read32(dev, B43_SHM_SHARED, shm_offset);
+        a = tmp & 0xFF;
+        b = (tmp >> 8) & 0xFF;
+        c = (tmp >> 16) & 0xFF;
+        d = (tmp >> 24) & 0xFF;
+        if (a == 0 || a == B43_TSSI_MAX ||
+            b == 0 || b == B43_TSSI_MAX ||
+            c == 0 || c == B43_TSSI_MAX ||
+            d == 0 || d == B43_TSSI_MAX)
+                return -ENOENT;
+        /* The values are OK. Clear them. */
+        tmp = B43_TSSI_MAX | (B43_TSSI_MAX << 8) |
+              (B43_TSSI_MAX << 16) | (B43_TSSI_MAX << 24);
+        b43_shm_write32(dev, B43_SHM_SHARED, shm_offset, tmp);
+
+        if (is_ofdm) {
+                a = (a + 32) & 0x3F;
+                b = (b + 32) & 0x3F;
+                c = (c + 32) & 0x3F;
+                d = (d + 32) & 0x3F;
+        }
+
+        /* Get the average of the values with 0.5 added to each value. */
+        average = (a + b + c + d + 2) / 4;
+        if (is_ofdm) {
+                /* Adjust for CCK-boost */
+                if (b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_HOSTFLO)
+                    & B43_HF_CCKBOOST)
+                        average = (average >= 13) ? (average - 13) : 0;
+        }
+
+        return average;
+}
diff --git a/drivers/net/wireless/b43/phy_common.h b/drivers/net/wireless/b43/phy_common.h
index 9b9635eda9c4..f8db9f40df5d 100644
--- a/drivers/net/wireless/b43/phy_common.h
+++ b/drivers/net/wireless/b43/phy_common.h
@@ -61,6 +61,17 @@ enum {
 };
 
 /**
+ * enum b43_txpwr_result - Return value for the recalc_txpower PHY op.
+ *
+ * @B43_TXPWR_RES_NEED_ADJUST:  Values changed. Hardware adjustment is needed.
+ * @B43_TXPWR_RES_DONE:         No more work to do. Everything is done.
+ */
+enum b43_txpwr_result {
+        B43_TXPWR_RES_NEED_ADJUST,
+        B43_TXPWR_RES_DONE,
+};
+
+/**
  * struct b43_phy_operations - Function pointers for PHY ops.
  *
  * @prepare:            Prepare the PHY. This is called before @init.
@@ -96,8 +107,23 @@ enum {
  * @interf_mitigation:  Switch the Interference Mitigation mode.
  *                      Can be NULL, if not supported.
  *
- * @xmitpower:          FIXME REMOVEME
+ * @recalc_txpower:     Recalculate the transmission power parameters.
+ *                      This callback has to recalculate the TX power settings,
+ *                      but does not need to write them to the hardware, yet.
+ *                      Returns enum b43_txpwr_result to indicate whether the hardware
+ *                      needs to be adjusted.
+ *                      If B43_TXPWR_NEED_ADJUST is returned, @adjust_txpower
+ *                      will be called later.
+ *                      If the parameter "ignore_tssi" is true, the TSSI values should
+ *                      be ignored and a recalculation of the power settings should be
+ *                      done even if the TSSI values did not change.
+ *                      This callback is called with wl->irq_lock held and must not sleep.
  *                      Must not be NULL.
+ * @adjust_txpower:     Write the previously calculated TX power settings
+ *                      (from @recalc_txpower) to the hardware.
+ *                      This function may sleep.
+ *                      Can be NULL, if (and ONLY if) @recalc_txpower _always_
+ *                      returns B43_TXPWR_RES_DONE.
  *
  * @pwork_15sec:        Periodic work. Called every 15 seconds.
  *                      Can be NULL, if not required.
@@ -127,7 +153,9 @@ struct b43_phy_operations {
                                  enum b43_interference_mitigation new_mode);
 
         /* Transmission power adjustment */
-        void (*xmitpower)(struct b43_wldev *dev);
+        enum b43_txpwr_result (*recalc_txpower)(struct b43_wldev *dev,
+                                                bool ignore_tssi);
+        void (*adjust_txpower)(struct b43_wldev *dev);
 
         /* Misc */
         void (*pwork_15sec)(struct b43_wldev *dev);
@@ -183,11 +211,15 @@ struct b43_phy {
 
         /* Desired TX power level (in dBm).
          * This is set by the user and adjusted in b43_phy_xmitpower(). */
-        u8 power_level;
+        int desired_txpower;
 
         /* Hardware Power Control enabled? */
         bool hardware_power_control;
 
+        /* The time (in absolute jiffies) when the next TX power output
+         * check is needed. */
+        unsigned long next_txpwr_check_time;
+
         /* current channel */
         unsigned int channel;
 
@@ -309,4 +341,41 @@ int b43_switch_channel(struct b43_wldev *dev, unsigned int new_channel);
  */
 void b43_software_rfkill(struct b43_wldev *dev, enum rfkill_state state);
 
+/**
+ * b43_phy_txpower_check - Check TX power output.
+ *
+ * Compare the current TX power output to the desired power emission
+ * and schedule an adjustment in case it mismatches.
+ * Requires wl->irq_lock locked.
+ *
+ * @flags:      OR'ed enum b43_phy_txpower_check_flags flags.
+ *              See the docs below.
+ */
+void b43_phy_txpower_check(struct b43_wldev *dev, unsigned int flags);
+/**
+ * enum b43_phy_txpower_check_flags - Flags for b43_phy_txpower_check()
+ *
+ * @B43_TXPWR_IGNORE_TIME: Ignore the schedule time and force-redo
+ *                         the check now.
+ * @B43_TXPWR_IGNORE_TSSI: Redo the recalculation, even if the average
+ *                         TSSI did not change.
+ */
+enum b43_phy_txpower_check_flags {
+        B43_TXPWR_IGNORE_TIME           = (1 << 0),
+        B43_TXPWR_IGNORE_TSSI           = (1 << 1),
+};
+
+struct work_struct;
+void b43_phy_txpower_adjust_work(struct work_struct *work);
+
+/**
+ * b43_phy_shm_tssi_read - Read the average of the last 4 TSSI from SHM.
+ *
+ * @shm_offset:         The SHM address to read the values from.
+ *
+ * Returns the average of the 4 TSSI values, or a negative error code.
+ */
+int b43_phy_shm_tssi_read(struct b43_wldev *dev, u16 shm_offset);
+
+
 #endif /* LINUX_B43_PHY_COMMON_H_ */
diff --git a/drivers/net/wireless/b43/phy_g.c b/drivers/net/wireless/b43/phy_g.c
index bb95c54cd43d..fce84896d34c 100644
--- a/drivers/net/wireless/b43/phy_g.c
+++ b/drivers/net/wireless/b43/phy_g.c
@@ -2659,6 +2659,7 @@ static int b43_gphy_op_allocate(struct b43_wldev *dev)
         /* OFDM-table address caching. */
         gphy->ofdmtab_addr_direction = B43_OFDMTAB_DIRECTION_UNKNOWN;
 
+        gphy->average_tssi = 0xFF;
 
         lo = kzalloc(sizeof(*lo), GFP_KERNEL);
         if (!lo) {
@@ -3011,113 +3012,20 @@ static void b43_put_attenuation_into_ranges(struct b43_wldev *dev,
         *_bbatt = clamp_val(bbatt, bb_min, bb_max);
 }
 
-static void b43_gphy_op_xmitpower(struct b43_wldev *dev)
+static void b43_gphy_op_adjust_txpower(struct b43_wldev *dev)
 {
-        struct ssb_bus *bus = dev->dev->bus;
         struct b43_phy *phy = &dev->phy;
         struct b43_phy_g *gphy = phy->g;
-        u16 tmp;
-        s8 v0, v1, v2, v3;
-        s8 average;
-        int max_pwr;
-        int desired_pwr, estimated_pwr, pwr_adjust;
-        int rfatt_delta, bbatt_delta;
         int rfatt, bbatt;
         u8 tx_control;
 
-        if (gphy->cur_idle_tssi == 0)
-                return;
-        if ((bus->boardinfo.vendor == SSB_BOARDVENDOR_BCM) &&
-            (bus->boardinfo.type == SSB_BOARD_BU4306))
-                return;
-
-        tmp = b43_shm_read16(dev, B43_SHM_SHARED, 0x0058);
-        v0 = (s8) (tmp & 0x00FF);
-        v1 = (s8) ((tmp & 0xFF00) >> 8);
-        tmp = b43_shm_read16(dev, B43_SHM_SHARED, 0x005A);
-        v2 = (s8) (tmp & 0x00FF);
-        v3 = (s8) ((tmp & 0xFF00) >> 8);
-        tmp = 0;
-
-        if (v0 == 0x7F || v1 == 0x7F || v2 == 0x7F
-            || v3 == 0x7F) {
-                tmp = b43_shm_read16(dev, B43_SHM_SHARED, 0x0070);
-                v0 = (s8) (tmp & 0x00FF);
-                v1 = (s8) ((tmp & 0xFF00) >> 8);
-                tmp = b43_shm_read16(dev, B43_SHM_SHARED, 0x0072);
-                v2 = (s8) (tmp & 0x00FF);
-                v3 = (s8) ((tmp & 0xFF00) >> 8);
-                if (v0 == 0x7F || v1 == 0x7F || v2 == 0x7F
-                    || v3 == 0x7F)
-                        return;
-                v0 = (v0 + 0x20) & 0x3F;
-                v1 = (v1 + 0x20) & 0x3F;
-                v2 = (v2 + 0x20) & 0x3F;
-                v3 = (v3 + 0x20) & 0x3F;
-                tmp = 1;
-        }
-        b43_shm_clear_tssi(dev);
-
-        average = (v0 + v1 + v2 + v3 + 2) / 4;
-
-        if (tmp && (b43_shm_read16(dev, B43_SHM_SHARED, 0x005E) & 0x8))
-                average -= 13;
-
-        estimated_pwr = b43_gphy_estimate_power_out(dev, average);
-
-        max_pwr = dev->dev->bus->sprom.maxpwr_bg;
-        if ((dev->dev->bus->sprom.boardflags_lo
-            & B43_BFL_PACTRL) && (phy->type == B43_PHYTYPE_G))
-                max_pwr -= 0x3;
-        if (unlikely(max_pwr <= 0)) {
-                b43warn(dev->wl,
-                        "Invalid max-TX-power value in SPROM.\n");
-                max_pwr = 60;   /* fake it */
-                dev->dev->bus->sprom.maxpwr_bg = max_pwr;
-        }
-
-        /*TODO:
-           max_pwr = min(REG - dev->dev->bus->sprom.antennagain_bgphy - 0x6, max_pwr)
-           where REG is the max power as per the regulatory domain
-         */
-
-        /* Get desired power (in Q5.2) */
-        desired_pwr = INT_TO_Q52(phy->power_level);
-        /* And limit it. max_pwr already is Q5.2 */
-        desired_pwr = clamp_val(desired_pwr, 0, max_pwr);
-        if (b43_debug(dev, B43_DBG_XMITPOWER)) {
-                b43dbg(dev->wl,
-                       "Current TX power output: " Q52_FMT
-                       " dBm, " "Desired TX power output: "
-                       Q52_FMT " dBm\n", Q52_ARG(estimated_pwr),
-                       Q52_ARG(desired_pwr));
-        }
-
-        /* Calculate the adjustment delta. */
-        pwr_adjust = desired_pwr - estimated_pwr;
-
-        /* RF attenuation delta. */
-        rfatt_delta = ((pwr_adjust + 7) / 8);
-        /* Lower attenuation => Bigger power output. Negate it. */
-        rfatt_delta = -rfatt_delta;
-
-        /* Baseband attenuation delta. */
-        bbatt_delta = pwr_adjust / 2;
-        /* Lower attenuation => Bigger power output. Negate it. */
-        bbatt_delta = -bbatt_delta;
-        /* RF att affects power level 4 times as much as
-         * Baseband attennuation. Subtract it. */
-        bbatt_delta -= 4 * rfatt_delta;
-
-        /* So do we finally need to adjust something? */
-        if ((rfatt_delta == 0) && (bbatt_delta == 0))
-                return;
+        spin_lock_irq(&dev->wl->irq_lock);
 
         /* Calculate the new attenuation values. */
         bbatt = gphy->bbatt.att;
-        bbatt += bbatt_delta;
+        bbatt += gphy->bbatt_delta;
         rfatt = gphy->rfatt.att;
-        rfatt += rfatt_delta;
+        rfatt += gphy->rfatt_delta;
 
         b43_put_attenuation_into_ranges(dev, &bbatt, &rfatt);
         tx_control = gphy->tx_control;
@@ -3152,6 +3060,14 @@ static void b43_gphy_op_xmitpower(struct b43_wldev *dev)
         gphy->rfatt.att = rfatt;
         gphy->bbatt.att = bbatt;
 
+        /* We drop the lock early, so we can sleep during hardware
+         * adjustment. Possible races with op_recalc_txpower are harmless,
+         * as we will be called once again in case we raced. */
+        spin_unlock_irq(&dev->wl->irq_lock);
+
+        if (b43_debug(dev, B43_DBG_XMITPOWER))
+                b43dbg(dev->wl, "Adjusting TX power\n");
+
         /* Adjust the hardware */
         b43_phy_lock(dev);
         b43_radio_lock(dev);
@@ -3161,6 +3077,111 @@ static void b43_gphy_op_xmitpower(struct b43_wldev *dev)
         b43_phy_unlock(dev);
 }
 
+static enum b43_txpwr_result b43_gphy_op_recalc_txpower(struct b43_wldev *dev,
+                                                        bool ignore_tssi)
+{
+        struct b43_phy *phy = &dev->phy;
+        struct b43_phy_g *gphy = phy->g;
+        unsigned int average_tssi;
+        int cck_result, ofdm_result;
+        int estimated_pwr, desired_pwr, pwr_adjust;
+        int rfatt_delta, bbatt_delta;
+        unsigned int max_pwr;
+
+        /* First get the average TSSI */
+        cck_result = b43_phy_shm_tssi_read(dev, B43_SHM_SH_TSSI_CCK);
+        ofdm_result = b43_phy_shm_tssi_read(dev, B43_SHM_SH_TSSI_OFDM_G);
+        if ((cck_result < 0) && (ofdm_result < 0)) {
+                /* No TSSI information available */
+                if (!ignore_tssi)
+                        goto no_adjustment_needed;
+                cck_result = 0;
+                ofdm_result = 0;
+        }
+        if (cck_result < 0)
+                average_tssi = ofdm_result;
+        else if (ofdm_result < 0)
+                average_tssi = cck_result;
+        else
+                average_tssi = (cck_result + ofdm_result) / 2;
+        /* Merge the average with the stored value. */
+        if (likely(gphy->average_tssi != 0xFF))
+                average_tssi = (average_tssi + gphy->average_tssi) / 2;
+        gphy->average_tssi = average_tssi;
+        B43_WARN_ON(average_tssi >= B43_TSSI_MAX);
+
+        /* Estimate the TX power emission based on the TSSI */
+        estimated_pwr = b43_gphy_estimate_power_out(dev, average_tssi);
+
+        B43_WARN_ON(phy->type != B43_PHYTYPE_G);
+        max_pwr = dev->dev->bus->sprom.maxpwr_bg;
+        if (dev->dev->bus->sprom.boardflags_lo & B43_BFL_PACTRL)
+                max_pwr -= 3; /* minus 0.75 */
+        if (unlikely(max_pwr >= INT_TO_Q52(30/*dBm*/))) {
+                b43warn(dev->wl,
+                        "Invalid max-TX-power value in SPROM.\n");
+                max_pwr = INT_TO_Q52(20); /* fake it */
+                dev->dev->bus->sprom.maxpwr_bg = max_pwr;
+        }
+
+        /* Get desired power (in Q5.2) */
+        if (phy->desired_txpower < 0)
+                desired_pwr = INT_TO_Q52(0);
+        else
+                desired_pwr = INT_TO_Q52(phy->desired_txpower);
+        /* And limit it. max_pwr already is Q5.2 */
+        desired_pwr = clamp_val(desired_pwr, 0, max_pwr);
+        if (b43_debug(dev, B43_DBG_XMITPOWER)) {
+                b43dbg(dev->wl,
+                       "[TX power]  current = " Q52_FMT
+                       " dBm,  desired = " Q52_FMT
+                       " dBm,  max = " Q52_FMT "\n",
+                       Q52_ARG(estimated_pwr),
+                       Q52_ARG(desired_pwr),
+                       Q52_ARG(max_pwr));
+        }
+
+        /* Calculate the adjustment delta. */
+        pwr_adjust = desired_pwr - estimated_pwr;
+        if (pwr_adjust == 0)
+                goto no_adjustment_needed;
+
+        /* RF attenuation delta. */
+        rfatt_delta = ((pwr_adjust + 7) / 8);
+        /* Lower attenuation => Bigger power output. Negate it. */
+        rfatt_delta = -rfatt_delta;
+
+        /* Baseband attenuation delta. */
+        bbatt_delta = pwr_adjust / 2;
+        /* Lower attenuation => Bigger power output. Negate it. */
+        bbatt_delta = -bbatt_delta;
+        /* RF att affects power level 4 times as much as
+         * Baseband attennuation. Subtract it. */
+        bbatt_delta -= 4 * rfatt_delta;
+
+        if (b43_debug(dev, B43_DBG_XMITPOWER)) {
+                int dbm = pwr_adjust < 0 ? -pwr_adjust : pwr_adjust;
+                b43dbg(dev->wl,
+                       "[TX power deltas]  %s" Q52_FMT " dBm   =>   "
+                       "bbatt-delta = %d,  rfatt-delta = %d\n",
+                       (pwr_adjust < 0 ? "-" : ""), Q52_ARG(dbm),
+                       bbatt_delta, rfatt_delta);
+        }
+        /* So do we finally need to adjust something in hardware? */
+        if ((rfatt_delta == 0) && (bbatt_delta == 0))
+                goto no_adjustment_needed;
+
+        /* Save the deltas for later when we adjust the power. */
+        gphy->bbatt_delta = bbatt_delta;
+        gphy->rfatt_delta = rfatt_delta;
+
+        /* We need to adjust the TX power on the device. */
+        return B43_TXPWR_RES_NEED_ADJUST;
+
+no_adjustment_needed:
+        return B43_TXPWR_RES_DONE;
+}
+
 static void b43_gphy_op_pwork_15sec(struct b43_wldev *dev)
 {
         struct b43_phy *phy = &dev->phy;
@@ -3223,7 +3244,8 @@ const struct b43_phy_operations b43_phyops_g = {
         .get_default_chan       = b43_gphy_op_get_default_chan,
         .set_rx_antenna         = b43_gphy_op_set_rx_antenna,
         .interf_mitigation      = b43_gphy_op_interf_mitigation,
-        .xmitpower              = b43_gphy_op_xmitpower,
+        .recalc_txpower         = b43_gphy_op_recalc_txpower,
+        .adjust_txpower         = b43_gphy_op_adjust_txpower,
         .pwork_15sec            = b43_gphy_op_pwork_15sec,
         .pwork_60sec            = b43_gphy_op_pwork_60sec,
 };
diff --git a/drivers/net/wireless/b43/phy_g.h b/drivers/net/wireless/b43/phy_g.h
index 1f0daebd6eb6..7f95edea1c63 100644
--- a/drivers/net/wireless/b43/phy_g.h
+++ b/drivers/net/wireless/b43/phy_g.h
@@ -115,7 +115,6 @@ struct b43_txpower_lo_control;
 
 struct b43_phy_g {
         bool initialised;
-        bool dyn_tssi_tbl;      /* tssi2dbm is kmalloc()ed. */
 
         /* ACI (adjacent channel interference) flags. */
         bool aci_enable;
@@ -135,12 +134,26 @@ struct b43_phy_g {
         u16 minlowsig[2];
         u16 minlowsigpos[2];
 
-        /* TSSI to dBm table in use */
+        /* Pointer to the table used to convert a
+         * TSSI value to dBm-Q5.2 */
         const s8 *tssi2dbm;
+        /* tssi2dbm is kmalloc()ed. Only used for free()ing. */
+        bool dyn_tssi_tbl;
         /* Target idle TSSI */
         int tgt_idle_tssi;
         /* Current idle TSSI */
         int cur_idle_tssi;
+        /* The current average TSSI.
+         * Needs irq_lock, as it's updated in the IRQ path. */
+        u8 average_tssi;
+        /* Current TX power level attenuation control values */
+        struct b43_bbatt bbatt;
+        struct b43_rfatt rfatt;
+        u8 tx_control;          /* B43_TXCTL_XXX */
+        /* The calculated attenuation deltas that are used later
+         * when adjusting the actual power output. */
+        int bbatt_delta;
+        int rfatt_delta;
 
         /* LocalOscillator control values. */
         struct b43_txpower_lo_control *lo_control;
@@ -151,11 +164,6 @@ struct b43_phy_g {
         s16 lna_gain;           /* LNA */
         s16 pga_gain;           /* PGA */
 
-        /* Current TX power level attenuation control values */
-        struct b43_bbatt bbatt;
-        struct b43_rfatt rfatt;
-        u8 tx_control;          /* B43_TXCTL_XXX */
-
         /* Current Interference Mitigation mode */
         int interfmode;
         /* Stack of saved values from the Interference Mitigation code.
diff --git a/drivers/net/wireless/b43/xmit.c b/drivers/net/wireless/b43/xmit.c
index c8a831234e4a..5e0b71c3ad02 100644
--- a/drivers/net/wireless/b43/xmit.c
+++ b/drivers/net/wireless/b43/xmit.c
@@ -680,6 +680,8 @@ void b43_handle_txstatus(struct b43_wldev *dev,
                 b43_pio_handle_txstatus(dev, status);
         else
                 b43_dma_handle_txstatus(dev, status);
+
+        b43_phy_txpower_check(dev, 0);
 }
 
 /* Fill out the mac80211 TXstatus report based on the b43-specific