1 files changed, 32 insertions, 11 deletions

diff --git a/drivers/bcma/scan.c b/drivers/bcma/scan.c
index cad994857683..f94cccccfa56 100644
--- a/drivers/bcma/scan.c
+++ b/drivers/bcma/scan.c
@@ -212,6 +212,17 @@ static struct bcma_device *bcma_find_core_by_index(struct bcma_bus *bus,
 	return NULL;
 }
 
+static struct bcma_device *bcma_find_core_reverse(struct bcma_bus *bus, u16 coreid)
+{
+	struct bcma_device *core;
+
+	list_for_each_entry_reverse(core, &bus->cores, list) {
+		if (core->id.id == coreid)
+			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)
@@ -353,6 +364,7 @@ static int bcma_get_next_core(struct bcma_bus *bus, u32 __iomem **eromptr,
 void bcma_init_bus(struct bcma_bus *bus)
 {
 	s32 tmp;
+	struct bcma_chipinfo *chipinfo = &(bus->chipinfo);
 
 	if (bus->init_done)
 		return;
@@ -363,9 +375,12 @@ void bcma_init_bus(struct bcma_bus *bus)
 	bcma_scan_switch_core(bus, BCMA_ADDR_BASE);
 
 	tmp = bcma_scan_read32(bus, 0, BCMA_CC_ID);
-	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;
+	chipinfo->id = (tmp & BCMA_CC_ID_ID) >> BCMA_CC_ID_ID_SHIFT;
+	chipinfo->rev = (tmp & BCMA_CC_ID_REV) >> BCMA_CC_ID_REV_SHIFT;
+	chipinfo->pkg = (tmp & BCMA_CC_ID_PKG) >> BCMA_CC_ID_PKG_SHIFT;
+	pr_info("Found chip with id 0x%04X, rev 0x%02X and package 0x%02X\n",
+		chipinfo->id, chipinfo->rev, chipinfo->pkg);
+
 	bus->init_done = true;
 }
 
@@ -392,6 +407,7 @@ int bcma_bus_scan(struct bcma_bus *bus)
 	bcma_scan_switch_core(bus, erombase);
 
 	while (eromptr < eromend) {
+		struct bcma_device *other_core;
 		struct bcma_device *core = kzalloc(sizeof(*core), GFP_KERNEL);
 		if (!core)
 			return -ENOMEM;
@@ -399,18 +415,23 @@ int bcma_bus_scan(struct bcma_bus *bus)
 		core->bus = bus;
 
 		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)
+		if (err < 0) {
+			kfree(core);
+			if (err == -ENODEV) {
+				core_num++;
+				continue;
+			} else if (err == -ENXIO) {
+				continue;
+			} else if (err == -ESPIPE) {
+				break;
+			}
 			return err;
+		}
 
 		core->core_index = core_num++;
 		bus->nr_cores++;
+		other_core = bcma_find_core_reverse(bus, core->id.id);
+		core->core_unit = (other_core == NULL) ? 0 : other_core->core_unit + 1;
 
 		pr_info("Core %d found: %s "
 			"(manuf 0x%03X, id 0x%03X, rev 0x%02X, class 0x%X)\n",