ssb: when needed, reject IM input while disabling device

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

1 files changed, 15 insertions, 1 deletions

diff --git a/drivers/ssb/main.c b/drivers/ssb/main.c
index 775c579817b4..06c0b6d5250d 100644
--- a/drivers/ssb/main.c
+++ b/drivers/ssb/main.c
@@ -1220,7 +1220,7 @@ static int ssb_wait_bits(struct ssb_device *dev, u16 reg, u32 bitmask,
 
 void ssb_device_disable(struct ssb_device *dev, u32 core_specific_flags)
 {
-        u32 reject;
+        u32 reject, val;
 
         if (ssb_read32(dev, SSB_TMSLOW) & SSB_TMSLOW_RESET)
                 return;
@@ -1229,12 +1229,26 @@ void ssb_device_disable(struct ssb_device *dev, u32 core_specific_flags)
         ssb_write32(dev, SSB_TMSLOW, reject | SSB_TMSLOW_CLOCK);
         ssb_wait_bits(dev, SSB_TMSLOW, reject, 1000, 1);
         ssb_wait_bits(dev, SSB_TMSHIGH, SSB_TMSHIGH_BUSY, 1000, 0);
+
+        if (ssb_read32(dev, SSB_IDLOW) & SSB_IDLOW_INITIATOR) {
+                val = ssb_read32(dev, SSB_IMSTATE);
+                val |= SSB_IMSTATE_REJECT;
+                ssb_write32(dev, SSB_IMSTATE, val);
+                ssb_wait_bits(dev, SSB_IMSTATE, SSB_IMSTATE_BUSY, 1000, 0);
+        }
+
         ssb_write32(dev, SSB_TMSLOW,
                     SSB_TMSLOW_FGC | SSB_TMSLOW_CLOCK |
                     reject | SSB_TMSLOW_RESET |
                     core_specific_flags);
         ssb_flush_tmslow(dev);
 
+        if (ssb_read32(dev, SSB_IDLOW) & SSB_IDLOW_INITIATOR) {
+                val = ssb_read32(dev, SSB_IMSTATE);
+                val &= ~SSB_IMSTATE_REJECT;
+                ssb_write32(dev, SSB_IMSTATE, val);
+        }
+
         ssb_write32(dev, SSB_TMSLOW,
                     reject | SSB_TMSLOW_RESET |
                     core_specific_flags);