b43: Add lpphy_update_tx_power_npt function to improve TX power handling on LP PHYs

diff -uNr compat-wireless-2009-09-15/drivers/net/wireless/b43/b43.h compat-wireless-2009-09-15.mod/drivers/net/wireless/b43/b43.h
@@ -255,7 +255,10 @@
 #define B43_SHM_SH_MAXBFRAMES		0x0080	/* Maximum number of frames in a burst */
 #define B43_SHM_SH_SPUWKUP		0x0094	/* pre-wakeup for synth PU in us */
 #define B43_SHM_SH_PRETBTT		0x0096	/* pre-TBTT in us */
-
+/* MAC Statistics */
+#define B43_SHM_SH_TX_FRAMES_SENT	0x00E0	/* # TX Frames sent */
+#define B43_SHM_SH_TX_RTS		0x00E2	/* # TX RTS */
+#define B43_SHM_SH_TX_CTS		0x00E4	/* # TX CTS */
 /* SHM_SCRATCH offsets */
 #define B43_SHM_SC_MINCONT		0x0003	/* Minimum contention window */
 #define B43_SHM_SC_MAXCONT		0x0004	/* Maximum contention window */

diff -uNr compat-wireless-2009-09-15/drivers/net/wireless/b43/phy_lp.c compat-wireless-2009-09-15.mod/drivers/net/wireless/b43/phy_lp.c
@@ -1103,6 +1103,28 @@
 			(u16)~B43_LPPHY_TX_PWR_CTL_CMD_MODE, ctl);
 }
 
+static void lpphy_update_tx_power_npt(struct b43_wldev *dev)
+{
+	u16 tx_cnt;
+	u16 npt;
+	u16 s3;
+
+	s3 = b43_shm_read16(dev, B43_SHM_SHARED, B43_SHM_SH_TX_FRAMES_SENT);
+	tx_cnt = s3 - dev->phy.lp->tssi_tx_count;
+	npt = (b43_phy_read(dev, B43_LPPHY_TX_PWR_CTL_NNUM) & 0x700) >> 8;
+
+	if ((1 << (npt & 0x1F)) >= tx_cnt)
+		return;
+	/* NB: Spec says do the stuff if xxx < tx_cnt, so return on xxx > txcnt */
+	dev->phy.lp->tssi_tx_count = s3;
+	if (npt < 7) {
+		npt++;
+		b43_phy_maskset(dev, B43_LPPHY_TX_PWR_CTL_NNUM, 0xF8FF, (npt << 8));
+	}
+	dev->phy.lp->tssi_npt = npt;
+	dev->phy.lp->tssi_idx = (b43_phy_read(dev, B43_LPPHY_TX_PWR_CTL_STAT) & 0x7F00) >> 8;
+}
+
 static void lpphy_set_tx_power_control(struct b43_wldev *dev,
 				       enum b43_lpphy_txpctl_mode mode)
 {

@@ -1116,7 +1138,7 @@
 	lpphy->txpctl_mode = mode;
 
 	if (oldmode == B43_LPPHY_TXPCTL_HW) {
-		//TODO Update TX Power NPT
+		lpphy_update_tx_power_npt(dev);
 		//TODO Clear all TX Power offsets
 	} else {
 		if (mode == B43_LPPHY_TXPCTL_HW) {