rt2x00: Fix beacon skew in rt2800pci

rt2800pci is suffering from beacon skew in AP mode. Some powersaving
clients (like VOIP phones) are getting into trouble after some time
when the beacon skew is getting too big.

The ralink legacy drivers contain a function that indicates that the
beacon timer is off by 1us per tbtt. And this function works around
that by reducing the beacon interval for every 64th beacon transmission
by 64us (the smallest possible value). Do the same in rt2800pci.

This allows proper powersaving when rt2800pci is used in AP mode.
Signed-off-by: Helmut Schaa <helmut.schaa@googlemail.com>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

drivers/net/wireless/rt2x00/rt2800.h

@@ -2474,6 +2474,12 @@ struct mac_iveiv_entry {
  */
 #define EIRP_MAX_TX_POWER_LIMIT	0x50
 
+/*
+ * Number of TBTT intervals after which we have to adjust
+ * the hw beacon timer.
+ */
+#define BCN_TBTT_OFFSET 64
+
 /*
  * RT2800 driver data structure
  */
@@ -2484,6 +2490,7 @@ struct rt2800_drv_data {
 	u8 bbp26;
 	u8 txmixer_gain_24g;
 	u8 txmixer_gain_5g;
+	unsigned int tbtt_tick;
 };
 
 #endif /* RT2800_H */

drivers/net/wireless/rt2x00/rt2800pci.c

@@ -809,7 +809,33 @@ static void rt2800pci_pretbtt_tasklet(unsigned long data)
 static void rt2800pci_tbtt_tasklet(unsigned long data)
 {
 	struct rt2x00_dev *rt2x00dev = (struct rt2x00_dev *)data;
+	struct rt2800_drv_data *drv_data = rt2x00dev->drv_data;
+	u32 reg;
+
 	rt2x00lib_beacondone(rt2x00dev);
+
+	if (rt2x00dev->intf_ap_count) {
+		/*
+		 * The rt2800pci hardware tbtt timer is off by 1us per tbtt
+		 * causing beacon skew and as a result causing problems with
+		 * some powersaving clients over time. Shorten the beacon
+		 * interval every 64 beacons by 64us to mitigate this effect.
+		 */
+		if (drv_data->tbtt_tick == (BCN_TBTT_OFFSET - 2)) {
+			rt2x00pci_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
+			rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_INTERVAL,
+					   (rt2x00dev->beacon_int * 16) - 1);
+			rt2x00pci_register_write(rt2x00dev, BCN_TIME_CFG, reg);
+		} else if (drv_data->tbtt_tick == (BCN_TBTT_OFFSET - 1)) {
+			rt2x00pci_register_read(rt2x00dev, BCN_TIME_CFG, &reg);
+			rt2x00_set_field32(&reg, BCN_TIME_CFG_BEACON_INTERVAL,
+					   (rt2x00dev->beacon_int * 16));
+			rt2x00pci_register_write(rt2x00dev, BCN_TIME_CFG, reg);
+		}
+		drv_data->tbtt_tick++;
+		drv_data->tbtt_tick %= BCN_TBTT_OFFSET;
+	}
+
 	if (test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
 		rt2800pci_enable_interrupt(rt2x00dev, INT_MASK_CSR_TBTT);
 }