bcma: add bcma_core_pci_fixcfg()

This code is based on code from pcicore_fixcfg() in brcmsmac. This
patch is part of the move of pci specific code from brcmsmac to bcma.

Signed-off-by: Hauke Mehrtens <hauke@hauke-m.de>
Tested-by: Arend van Spriel <arend@broadcom.com>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

1 files changed, 19 insertions, 0 deletions

diff --git a/drivers/bcma/driver_pci.c b/drivers/bcma/driver_pci.c
index 949206674dc8..472d14fad643 100644
--- a/drivers/bcma/driver_pci.c
+++ b/drivers/bcma/driver_pci.c
@@ -168,12 +168,31 @@ static void bcma_pcicore_serdes_workaround(struct bcma_drv_pci *pc)
                                      tmp & ~BCMA_CORE_PCI_PLL_CTRL_FREQDET_EN);
 }
 
+static void bcma_core_pci_fixcfg(struct bcma_drv_pci *pc)
+{
+        struct bcma_device *core = pc->core;
+        u16 val16, core_index;
+        uint regoff;
+
+        regoff = BCMA_CORE_PCI_SPROM(BCMA_CORE_PCI_SPROM_PI_OFFSET);
+        core_index = (u16)core->core_index;
+
+        val16 = pcicore_read16(pc, regoff);
+        if (((val16 & BCMA_CORE_PCI_SPROM_PI_MASK) >> BCMA_CORE_PCI_SPROM_PI_SHIFT)
+             != core_index) {
+                val16 = (core_index << BCMA_CORE_PCI_SPROM_PI_SHIFT) |
+                        (val16 & ~BCMA_CORE_PCI_SPROM_PI_MASK);
+                pcicore_write16(pc, regoff, val16);
+        }
+}
+
 /**************************************************
  * Init.
  **************************************************/
 
 static void __devinit bcma_core_pci_clientmode_init(struct bcma_drv_pci *pc)
 {
+        bcma_core_pci_fixcfg(pc);
         bcma_pcicore_serdes_workaround(pc);
 }