ssb: pci: early fix for SPROM core index

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

diff --git a/drivers/ssb/driver_pcicore.c b/drivers/ssb/driver_pcicore.c
index 32a9b61f008dc75d6f949b65c85a35eeec12596a..8fde1220bc89e1c11528ad9b8f5d68196f38c72e 100644 (file)
--- a/drivers/ssb/driver_pcicore.c
+++ b/drivers/ssb/driver_pcicore.c
@@ -414,6 +414,16 @@ static int pcicore_is_in_hostmode(struct ssb_pcicore *pc)
  * Workarounds.
  **************************************************/
 
+static void ssb_pcicore_fix_sprom_core_index(struct ssb_pcicore *pc)
+{
+       u16 tmp = pcicore_read16(pc, SSB_PCICORE_SPROM(0));
+       if (((tmp & 0xF000) >> 12) != pc->dev->core_index) {
+               tmp &= ~0xF000;
+               tmp |= (pc->dev->core_index << 12);
+               pcicore_write16(pc, SSB_PCICORE_SPROM(0), tmp);
+       }
+}
+
 static u8 ssb_pcicore_polarity_workaround(struct ssb_pcicore *pc)
 {
        return (ssb_pcie_read(pc, 0x204) & 0x10) ? 0xC0 : 0x80;
@@ -521,6 +531,8 @@ void ssb_pcicore_init(struct ssb_pcicore *pc)
        if (!ssb_device_is_enabled(dev))
                ssb_device_enable(dev, 0);
 
+       ssb_pcicore_fix_sprom_core_index(pc);
+
 #ifdef CONFIG_SSB_PCICORE_HOSTMODE
        pc->hostmode = pcicore_is_in_hostmode(pc);
        if (pc->hostmode)