1 files changed, 237 insertions, 111 deletions

diff --git a/drivers/bcma/scan.c b/drivers/bcma/scan.c
index 40d7dcce8933..0ea390f9aa9e 100644
--- a/drivers/bcma/scan.c
+++ b/drivers/bcma/scan.c
@@ -200,18 +200,162 @@ static s32 bcma_erom_get_addr_desc(struct bcma_bus *bus, u32 **eromptr,
         return addrl;
 }
 
-int bcma_bus_scan(struct bcma_bus *bus)
+static struct bcma_device *bcma_find_core_by_index(struct bcma_bus *bus,
+                                                   u16 index)
 {
-        u32 erombase;
-        u32 __iomem *eromptr, *eromend;
+        struct bcma_device *core;
 
+        list_for_each_entry(core, &bus->cores, list) {
+                if (core->core_index == index)
+                        return core;
+        }
+        return NULL;
+}
+
+static int bcma_get_next_core(struct bcma_bus *bus, u32 __iomem **eromptr,
+                              struct bcma_device_id *match, int core_num,
+                              struct bcma_device *core)
+{
+        s32 tmp;
+        u8 i, j;
         s32 cia, cib;
         u8 ports[2], wrappers[2];
 
+        /* get CIs */
+        cia = bcma_erom_get_ci(bus, eromptr);
+        if (cia < 0) {
+                bcma_erom_push_ent(eromptr);
+                if (bcma_erom_is_end(bus, eromptr))
+                        return -ESPIPE;
+                return -EILSEQ;
+        }
+        cib = bcma_erom_get_ci(bus, eromptr);
+        if (cib < 0)
+                return -EILSEQ;
+
+        /* parse CIs */
+        core->id.class = (cia & SCAN_CIA_CLASS) >> SCAN_CIA_CLASS_SHIFT;
+        core->id.id = (cia & SCAN_CIA_ID) >> SCAN_CIA_ID_SHIFT;
+        core->id.manuf = (cia & SCAN_CIA_MANUF) >> SCAN_CIA_MANUF_SHIFT;
+        ports[0] = (cib & SCAN_CIB_NMP) >> SCAN_CIB_NMP_SHIFT;
+        ports[1] = (cib & SCAN_CIB_NSP) >> SCAN_CIB_NSP_SHIFT;
+        wrappers[0] = (cib & SCAN_CIB_NMW) >> SCAN_CIB_NMW_SHIFT;
+        wrappers[1] = (cib & SCAN_CIB_NSW) >> SCAN_CIB_NSW_SHIFT;
+        core->id.rev = (cib & SCAN_CIB_REV) >> SCAN_CIB_REV_SHIFT;
+
+        if (((core->id.manuf == BCMA_MANUF_ARM) &&
+             (core->id.id == 0xFFF)) ||
+            (ports[1] == 0)) {
+                bcma_erom_skip_component(bus, eromptr);
+                return -ENXIO;
+        }
+
+        /* check if component is a core at all */
+        if (wrappers[0] + wrappers[1] == 0) {
+                /* we could save addrl of the router
+                if (cid == BCMA_CORE_OOB_ROUTER)
+                 */
+                bcma_erom_skip_component(bus, eromptr);
+                return -ENXIO;
+        }
+
+        if (bcma_erom_is_bridge(bus, eromptr)) {
+                bcma_erom_skip_component(bus, eromptr);
+                return -ENXIO;
+        }
+
+        if (bcma_find_core_by_index(bus, core_num)) {
+                bcma_erom_skip_component(bus, eromptr);
+                return -ENODEV;
+        }
+
+        if (match && ((match->manuf != BCMA_ANY_MANUF &&
+              match->manuf != core->id.manuf) ||
+             (match->id != BCMA_ANY_ID && match->id != core->id.id) ||
+             (match->rev != BCMA_ANY_REV && match->rev != core->id.rev) ||
+             (match->class != BCMA_ANY_CLASS && match->class != core->id.class)
+            )) {
+                bcma_erom_skip_component(bus, eromptr);
+                return -ENODEV;
+        }
+
+        /* get & parse master ports */
+        for (i = 0; i < ports[0]; i++) {
+                u32 mst_port_d = bcma_erom_get_mst_port(bus, eromptr);
+                if (mst_port_d < 0)
+                        return -EILSEQ;
+        }
+
+        /* get & parse slave ports */
+        for (i = 0; i < ports[1]; i++) {
+                for (j = 0; ; j++) {
+                        tmp = bcma_erom_get_addr_desc(bus, eromptr,
+                                SCAN_ADDR_TYPE_SLAVE, i);
+                        if (tmp < 0) {
+                                /* no more entries for port _i_ */
+                                /* pr_debug("erom: slave port %d "
+                                 * "has %d descriptors\n", i, j); */
+                                break;
+                        } else {
+                                if (i == 0 && j == 0)
+                                        core->addr = tmp;
+                        }
+                }
+        }
+
+        /* get & parse master wrappers */
+        for (i = 0; i < wrappers[0]; i++) {
+                for (j = 0; ; j++) {
+                        tmp = bcma_erom_get_addr_desc(bus, eromptr,
+                                SCAN_ADDR_TYPE_MWRAP, i);
+                        if (tmp < 0) {
+                                /* no more entries for port _i_ */
+                                /* pr_debug("erom: master wrapper %d "
+                                 * "has %d descriptors\n", i, j); */
+                                break;
+                        } else {
+                                if (i == 0 && j == 0)
+                                        core->wrap = tmp;
+                        }
+                }
+        }
+
+        /* get & parse slave wrappers */
+        for (i = 0; i < wrappers[1]; i++) {
+                u8 hack = (ports[1] == 1) ? 0 : 1;
+                for (j = 0; ; j++) {
+                        tmp = bcma_erom_get_addr_desc(bus, eromptr,
+                                SCAN_ADDR_TYPE_SWRAP, i + hack);
+                        if (tmp < 0) {
+                                /* no more entries for port _i_ */
+                                /* pr_debug("erom: master wrapper %d "
+                                 * has %d descriptors\n", i, j); */
+                                break;
+                        } else {
+                                if (wrappers[0] == 0 && !i && !j)
+                                        core->wrap = tmp;
+                        }
+                }
+        }
+        if (bus->hosttype == BCMA_HOSTTYPE_SOC) {
+                core->io_addr = ioremap_nocache(core->addr, BCMA_CORE_SIZE);
+                if (!core->io_addr)
+                        return -ENOMEM;
+                core->io_wrap = ioremap_nocache(core->wrap, BCMA_CORE_SIZE);
+                if (!core->io_wrap) {
+                        iounmap(core->io_addr);
+                        return -ENOMEM;
+                }
+        }
+        return 0;
+}
+
+void bcma_init_bus(struct bcma_bus *bus)
+{
         s32 tmp;
-        u8 i, j;
 
-        int err;
+        if (bus->init_done)
+                return;
 
         INIT_LIST_HEAD(&bus->cores);
         bus->nr_cores = 0;
@@ -222,9 +366,27 @@ int bcma_bus_scan(struct bcma_bus *bus)
         bus->chipinfo.id = (tmp & BCMA_CC_ID_ID) >> BCMA_CC_ID_ID_SHIFT;
         bus->chipinfo.rev = (tmp & BCMA_CC_ID_REV) >> BCMA_CC_ID_REV_SHIFT;
         bus->chipinfo.pkg = (tmp & BCMA_CC_ID_PKG) >> BCMA_CC_ID_PKG_SHIFT;
+        bus->init_done = true;
+}
+
+int bcma_bus_scan(struct bcma_bus *bus)
+{
+        u32 erombase;
+        u32 __iomem *eromptr, *eromend;
+
+        int err, core_num = 0;
+
+        bcma_init_bus(bus);
 
         erombase = bcma_scan_read32(bus, 0, BCMA_CC_EROM);
-        eromptr = bus->mmio;
+        if (bus->hosttype == BCMA_HOSTTYPE_SOC) {
+                eromptr = ioremap_nocache(erombase, BCMA_CORE_SIZE);
+                if (!eromptr)
+                        return -ENOMEM;
+        } else {
+                eromptr = bus->mmio;
+        }
+
         eromend = eromptr + BCMA_CORE_SIZE / sizeof(u32);
 
         bcma_scan_switch_core(bus, erombase);
@@ -236,125 +398,89 @@ int bcma_bus_scan(struct bcma_bus *bus)
                 INIT_LIST_HEAD(&core->list);
                 core->bus = bus;
 
-                /* get CIs */
-                cia = bcma_erom_get_ci(bus, &eromptr);
-                if (cia < 0) {
-                        bcma_erom_push_ent(&eromptr);
-                        if (bcma_erom_is_end(bus, &eromptr))
-                                break;
-                        err= -EILSEQ;
-                        goto out;
-                }
-                cib = bcma_erom_get_ci(bus, &eromptr);
-                if (cib < 0) {
-                        err= -EILSEQ;
-                        goto out;
-                }
-
-                /* parse CIs */
-                core->id.class = (cia & SCAN_CIA_CLASS) >> SCAN_CIA_CLASS_SHIFT;
-                core->id.id = (cia & SCAN_CIA_ID) >> SCAN_CIA_ID_SHIFT;
-                core->id.manuf = (cia & SCAN_CIA_MANUF) >> SCAN_CIA_MANUF_SHIFT;
-                ports[0] = (cib & SCAN_CIB_NMP) >> SCAN_CIB_NMP_SHIFT;
-                ports[1] = (cib & SCAN_CIB_NSP) >> SCAN_CIB_NSP_SHIFT;
-                wrappers[0] = (cib & SCAN_CIB_NMW) >> SCAN_CIB_NMW_SHIFT;
-                wrappers[1] = (cib & SCAN_CIB_NSW) >> SCAN_CIB_NSW_SHIFT;
-                core->id.rev = (cib & SCAN_CIB_REV) >> SCAN_CIB_REV_SHIFT;
-
-                if (((core->id.manuf == BCMA_MANUF_ARM) &&
-                     (core->id.id == 0xFFF)) ||
-                    (ports[1] == 0)) {
-                        bcma_erom_skip_component(bus, &eromptr);
+                err = bcma_get_next_core(bus, &eromptr, NULL, core_num, core);
+                if (err == -ENODEV) {
+                        core_num++;
                         continue;
-                }
-
-                /* check if component is a core at all */
-                if (wrappers[0] + wrappers[1] == 0) {
-                        /* we could save addrl of the router
-                        if (cid == BCMA_CORE_OOB_ROUTER)
-                         */
-                        bcma_erom_skip_component(bus, &eromptr);
+                } else if (err == -ENXIO)
                         continue;
-                }
+                else if (err == -ESPIPE)
+                        break;
+                else if (err < 0)
+                        return err;
 
-                if (bcma_erom_is_bridge(bus, &eromptr)) {
-                        bcma_erom_skip_component(bus, &eromptr);
-                        continue;
-                }
+                core->core_index = core_num++;
+                bus->nr_cores++;
 
-                /* get & parse master ports */
-                for (i = 0; i < ports[0]; i++) {
-                        u32 mst_port_d = bcma_erom_get_mst_port(bus, &eromptr);
-                        if (mst_port_d < 0) {
-                                err= -EILSEQ;
-                                goto out;
-                        }
-                }
+                pr_info("Core %d found: %s "
+                        "(manuf 0x%03X, id 0x%03X, rev 0x%02X, class 0x%X)\n",
+                        core->core_index, bcma_device_name(&core->id),
+                        core->id.manuf, core->id.id, core->id.rev,
+                        core->id.class);
 
-                /* get & parse slave ports */
-                for (i = 0; i < ports[1]; i++) {
-                        for (j = 0; ; j++) {
-                                tmp = bcma_erom_get_addr_desc(bus, &eromptr,
-                                        SCAN_ADDR_TYPE_SLAVE, i);
-                                if (tmp < 0) {
-                                        /* no more entries for port _i_ */
-                                        /* pr_debug("erom: slave port %d "
-                                         * "has %d descriptors\n", i, j); */
-                                        break;
-                                } else {
-                                        if (i == 0 && j == 0)
-                                                core->addr = tmp;
-                                }
-                        }
-                }
+                list_add(&core->list, &bus->cores);
+        }
 
-                /* get & parse master wrappers */
-                for (i = 0; i < wrappers[0]; i++) {
-                        for (j = 0; ; j++) {
-                                tmp = bcma_erom_get_addr_desc(bus, &eromptr,
-                                        SCAN_ADDR_TYPE_MWRAP, i);
-                                if (tmp < 0) {
-                                        /* no more entries for port _i_ */
-                                        /* pr_debug("erom: master wrapper %d "
-                                         * "has %d descriptors\n", i, j); */
-                                        break;
-                                } else {
-                                        if (i == 0 && j == 0)
-                                                core->wrap = tmp;
-                                }
-                        }
-                }
+        if (bus->hosttype == BCMA_HOSTTYPE_SOC)
+                iounmap(eromptr);
 
-                /* get & parse slave wrappers */
-                for (i = 0; i < wrappers[1]; i++) {
-                        u8 hack = (ports[1] == 1) ? 0 : 1;
-                        for (j = 0; ; j++) {
-                                tmp = bcma_erom_get_addr_desc(bus, &eromptr,
-                                        SCAN_ADDR_TYPE_SWRAP, i + hack);
-                                if (tmp < 0) {
-                                        /* no more entries for port _i_ */
-                                        /* pr_debug("erom: master wrapper %d "
-                                         * has %d descriptors\n", i, j); */
-                                        break;
-                                } else {
-                                        if (wrappers[0] == 0 && !i && !j)
-                                                core->wrap = tmp;
-                                }
-                        }
-                }
+        return 0;
+}
+
+int __init bcma_bus_scan_early(struct bcma_bus *bus,
+                               struct bcma_device_id *match,
+                               struct bcma_device *core)
+{
+        u32 erombase;
+        u32 __iomem *eromptr, *eromend;
+
+        int err = -ENODEV;
+        int core_num = 0;
+
+        erombase = bcma_scan_read32(bus, 0, BCMA_CC_EROM);
+        if (bus->hosttype == BCMA_HOSTTYPE_SOC) {
+                eromptr = ioremap_nocache(erombase, BCMA_CORE_SIZE);
+                if (!eromptr)
+                        return -ENOMEM;
+        } else {
+                eromptr = bus->mmio;
+        }
+
+        eromend = eromptr + BCMA_CORE_SIZE / sizeof(u32);
+
+        bcma_scan_switch_core(bus, erombase);
+
+        while (eromptr < eromend) {
+                memset(core, 0, sizeof(*core));
+                INIT_LIST_HEAD(&core->list);
+                core->bus = bus;
 
+                err = bcma_get_next_core(bus, &eromptr, match, core_num, core);
+                if (err == -ENODEV) {
+                        core_num++;
+                        continue;
+                } else if (err == -ENXIO)
+                        continue;
+                else if (err == -ESPIPE)
+                        break;
+                else if (err < 0)
+                        return err;
+
+                core->core_index = core_num++;
+                bus->nr_cores++;
                 pr_info("Core %d found: %s "
                         "(manuf 0x%03X, id 0x%03X, rev 0x%02X, class 0x%X)\n",
-                        bus->nr_cores, bcma_device_name(&core->id),
+                        core->core_index, bcma_device_name(&core->id),
                         core->id.manuf, core->id.id, core->id.rev,
                         core->id.class);
 
-                core->core_index = bus->nr_cores++;
                 list_add(&core->list, &bus->cores);
-                continue;
-out:
-                return err;
+                err = 0;
+                break;
         }
 
-        return 0;
+        if (bus->hosttype == BCMA_HOSTTYPE_SOC)
+                iounmap(eromptr);
+
+        return err;
 }