b43: HT-PHY: Set MAC frequency to correct values

I misunderstood original Broadcom comment and used wrong values.
Signed-off-by: Rafał Miłecki <zajec5@gmail.com>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

drivers/net/wireless/b43/main.c

@@ -2985,7 +2985,22 @@ void b43_mac_switch_freq(struct b43_wldev *dev, u8 spurmode)
 {
 	u16 chip_id = dev->dev->chip_id;
 
-	if (chip_id == BCMA_CHIP_ID_BCM43131 ||
+	if (chip_id == BCMA_CHIP_ID_BCM4331) {
+		switch (spurmode) {
+		case 2: /* 168 Mhz: 2^26/168 = 0x61862 */
+			b43_write16(dev, B43_MMIO_TSF_CLK_FRAC_LOW, 0x1862);
+			b43_write16(dev, B43_MMIO_TSF_CLK_FRAC_HIGH, 0x6);
+			break;
+		case 1: /* 164 Mhz: 2^26/164 = 0x63e70 */
+			b43_write16(dev, B43_MMIO_TSF_CLK_FRAC_LOW, 0x3e70);
+			b43_write16(dev, B43_MMIO_TSF_CLK_FRAC_HIGH, 0x6);
+			break;
+		default: /* 160 Mhz: 2^26/160 = 0x66666 */
+			b43_write16(dev, B43_MMIO_TSF_CLK_FRAC_LOW, 0x6666);
+			b43_write16(dev, B43_MMIO_TSF_CLK_FRAC_HIGH, 0x6);
+			break;
+		}
+	} else if (chip_id == BCMA_CHIP_ID_BCM43131 ||
 	    chip_id == BCMA_CHIP_ID_BCM43217 ||
 	    chip_id == BCMA_CHIP_ID_BCM43222 ||
 	    chip_id == BCMA_CHIP_ID_BCM43224 ||

drivers/net/wireless/b43/phy_ht.c

@@ -728,7 +728,6 @@ static void b43_phy_ht_spur_avoid(struct b43_wldev *dev,
 {
 	struct bcma_device *core = dev->dev->bdev;
 	int spuravoid = 0;
-	u16 tmp;
 
 	/* Check for 13 and 14 is just a guess, we don't have enough logs. */
 	if (new_channel->hw_value == 13 || new_channel->hw_value == 14)
@@ -741,20 +740,7 @@ static void b43_phy_ht_spur_avoid(struct b43_wldev *dev,
 			  B43_BCMA_CLKCTLST_80211_PLL_ST |
 			  B43_BCMA_CLKCTLST_PHY_PLL_ST, false);
 
-	/* Values has been taken from wlc_bmac_switch_macfreq comments */
-	switch (spuravoid) {
-	case 2: /* 126MHz */
-		tmp = 0x2082;
-		break;
-	case 1: /* 123MHz */
-		tmp = 0x5341;
-		break;
-	default: /* 120MHz */
-		tmp = 0x8889;
-	}
-
-	b43_write16(dev, B43_MMIO_TSF_CLK_FRAC_LOW, tmp);
-	b43_write16(dev, B43_MMIO_TSF_CLK_FRAC_HIGH, 0x8);
+	b43_mac_switch_freq(dev, spuravoid);
 
 	/* TODO: reset PLL */