b43: (b2062) Fix crystal frequency calculations

This fixes the crystal frequency calculations in the b2062 init code.

Signed-off-by: Michael Buesch <mb@bu3sch.de>
Signed-off-by: John W. Linville <linville@tuxdriver.com>
diff --git a/drivers/net/wireless/b43/phy_lp.c b/drivers/net/wireless/b43/phy_lp.c
index fdd31d3..94d9e8b 100644
--- a/drivers/net/wireless/b43/phy_lp.c
+++ b/drivers/net/wireless/b43/phy_lp.c
@@ -160,6 +160,7 @@
 /* Initialize the 2062 radio. */
 static void lpphy_2062_init(struct b43_wldev *dev)
 {
+	struct ssb_bus *bus = dev->dev->bus;
 	u32 crystalfreq, pdiv, tmp, ref;
 	unsigned int i;
 	const struct b2062_freqdata *fd = NULL;
@@ -193,7 +194,11 @@
 	else
 		b43_radio_mask(dev, B2062_N_TSSI_CTL0, ~0x1);
 
-	crystalfreq = 0;//FIXME
+	/* Get the crystal freq, in Hz. */
+	crystalfreq = bus->chipco.pmu.crystalfreq * 1000;
+
+	B43_WARN_ON(!(bus->chipco.capabilities & SSB_CHIPCO_CAP_PMU));
+	B43_WARN_ON(crystalfreq == 0);
 
 	if (crystalfreq >= 30000000) {
 		pdiv = 1;
@@ -219,13 +224,15 @@
 			break;
 		}
 	}
-	if (B43_WARN_ON(!fd))
-		return;
+	if (!fd)
+		fd = &freqdata_tab[ARRAY_SIZE(freqdata_tab) - 1];
+	b43dbg(dev->wl, "b2062: Using crystal tab entry %u kHz.\n",
+	       fd->freq); /* FIXME: Keep this printk until the code is fully debugged. */
 
 	b43_radio_write(dev, B2062_S_RFPLL_CTL8,
 			((u16)(fd->data[1]) << 4) | fd->data[0]);
 	b43_radio_write(dev, B2062_S_RFPLL_CTL9,
-			((u16)(fd->data[3]) << 4) | fd->data[2]);//FIXME specs are different
+			((u16)(fd->data[3]) << 4) | fd->data[2]);
 	b43_radio_write(dev, B2062_S_RFPLL_CTL10, fd->data[4]);
 	b43_radio_write(dev, B2062_S_RFPLL_CTL11, fd->data[5]);
 }