b43: bcma: implement full core reset

Signed-off-by: Rafał Miłecki <zajec5@gmail.com>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

1 files changed, 27 insertions, 7 deletions

diff --git a/drivers/net/wireless/b43/main.c b/drivers/net/wireless/b43/main.c
index 15ff2187cce2..269483a52b3c 100644
--- a/drivers/net/wireless/b43/main.c
+++ b/drivers/net/wireless/b43/main.c
@@ -1156,17 +1156,37 @@ void b43_power_saving_ctl_bits(struct b43_wldev *dev, unsigned int ps_flags)
 }
 
 #ifdef CONFIG_B43_BCMA
-static void b43_bcma_wireless_core_reset(struct b43_wldev *dev, bool gmode)
+static void b43_bcma_phy_reset(struct b43_wldev *dev)
 {
-        u32 flags = 0;
+        u32 flags;
 
-        if (gmode)
-                flags = B43_BCMA_IOCTL_GMODE;
-        flags |= B43_BCMA_IOCTL_PHY_CLKEN;
+        /* Put PHY into reset */
+        flags = bcma_aread32(dev->dev->bdev, BCMA_IOCTL);
+        flags |= B43_BCMA_IOCTL_PHY_RESET;
         flags |= B43_BCMA_IOCTL_PHY_BW_20MHZ; /* Make 20 MHz def */
-        b43_device_enable(dev, flags);
+        bcma_awrite32(dev->dev->bdev, BCMA_IOCTL, flags);
+        udelay(2);
+
+        /* Take PHY out of reset */
+        flags = bcma_aread32(dev->dev->bdev, BCMA_IOCTL);
+        flags &= ~B43_BCMA_IOCTL_PHY_RESET;
+        flags |= BCMA_IOCTL_FGC;
+        bcma_awrite32(dev->dev->bdev, BCMA_IOCTL, flags);
+        udelay(1);
 
-        /* TODO: reset PHY */
+        /* Do not force clock anymore */
+        flags = bcma_aread32(dev->dev->bdev, BCMA_IOCTL);
+        flags &= ~BCMA_IOCTL_FGC;
+        bcma_awrite32(dev->dev->bdev, BCMA_IOCTL, flags);
+        udelay(1);
+}
+
+static void b43_bcma_wireless_core_reset(struct b43_wldev *dev, bool gmode)
+{
+        b43_device_enable(dev, B43_BCMA_IOCTL_PHY_CLKEN);
+        bcma_core_set_clockmode(dev->dev->bdev, BCMA_CLKMODE_FAST);
+        b43_bcma_phy_reset(dev);
+        bcma_core_pll_ctl(dev->dev->bdev, 0x300, 0x3000000, true);
 }
 #endif