1 files changed, 89 insertions, 6 deletions

diff --git a/drivers/bcma/scan.c b/drivers/bcma/scan.c
index 79705534217e..bf9f80652773 100644
--- a/drivers/bcma/scan.c
+++ b/drivers/bcma/scan.c
@@ -200,7 +200,20 @@ static s32 bcma_erom_get_addr_desc(struct bcma_bus *bus, u32 **eromptr,
 	return addrl;
 }
 
+static struct bcma_device *bcma_find_core_by_index(struct bcma_bus *bus,
+						   u16 index)
+{
+	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;
@@ -251,6 +264,21 @@ static int bcma_get_next_core(struct bcma_bus *bus, u32 __iomem **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);
@@ -312,10 +340,13 @@ static int bcma_get_next_core(struct bcma_bus *bus, u32 __iomem **eromptr,
 	return 0;
 }
 
-static void bcma_init_bus(struct bcma_bus *bus)
+void bcma_init_bus(struct bcma_bus *bus)
 {
 	s32 tmp;
 
+	if (bus->init_done)
+		return;
+
 	INIT_LIST_HEAD(&bus->cores);
 	bus->nr_cores = 0;
 
@@ -325,6 +356,7 @@ static void bcma_init_bus(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)
@@ -332,7 +364,7 @@ int bcma_bus_scan(struct bcma_bus *bus)
 	u32 erombase;
 	u32 __iomem *eromptr, *eromend;
 
-	int err;
+	int err, core_num = 0;
 
 	bcma_init_bus(bus);
 
@@ -349,23 +381,74 @@ int bcma_bus_scan(struct bcma_bus *bus)
 		INIT_LIST_HEAD(&core->list);
 		core->bus = bus;
 
-		err = bcma_get_next_core(bus, &eromptr, core);
-		if (err == -ENXIO)
+		err = bcma_get_next_core(bus, &eromptr, NULL, 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);
 	}
 
 	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, core_num = 0;
+
+	erombase = bcma_scan_read32(bus, 0, BCMA_CC_EROM);
+	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",
+			core->core_index, bcma_device_name(&core->id),
+			core->id.manuf, core->id.id, core->id.rev,
+			core->id.class);
+
+		list_add(&core->list, &bus->cores);
+		return 0;
+	}
+
+	return -ENODEV;
+}