diff options
Diffstat (limited to 'drivers/ssb/main.c')
| -rw-r--r-- | drivers/ssb/main.c | 44 |
1 files changed, 31 insertions, 13 deletions
diff --git a/drivers/ssb/main.c b/drivers/ssb/main.c index 3918d2cc585..e05ba6eefc7 100644 --- a/drivers/ssb/main.c +++ b/drivers/ssb/main.c @@ -1192,10 +1192,10 @@ void ssb_device_enable(struct ssb_device *dev, u32 core_specific_flags) } EXPORT_SYMBOL(ssb_device_enable); -/* Wait for a bit in a register to get set or unset. +/* Wait for bitmask in a register to get set or cleared. * timeout is in units of ten-microseconds */ -static int ssb_wait_bit(struct ssb_device *dev, u16 reg, u32 bitmask, - int timeout, int set) +static int ssb_wait_bits(struct ssb_device *dev, u16 reg, u32 bitmask, + int timeout, int set) { int i; u32 val; @@ -1203,7 +1203,7 @@ static int ssb_wait_bit(struct ssb_device *dev, u16 reg, u32 bitmask, for (i = 0; i < timeout; i++) { val = ssb_read32(dev, reg); if (set) { - if (val & bitmask) + if ((val & bitmask) == bitmask) return 0; } else { if (!(val & bitmask)) @@ -1220,20 +1220,38 @@ static int ssb_wait_bit(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; reject = ssb_tmslow_reject_bitmask(dev); - ssb_write32(dev, SSB_TMSLOW, reject | SSB_TMSLOW_CLOCK); - ssb_wait_bit(dev, SSB_TMSLOW, reject, 1000, 1); - ssb_wait_bit(dev, SSB_TMSHIGH, SSB_TMSHIGH_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_TMSLOW) & SSB_TMSLOW_CLOCK) { + 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 | |