diff --git a/src/adiv5.c b/src/adiv5.c
index 4bc64bb1..fdc26e2e 100644
--- a/src/adiv5.c
+++ b/src/adiv5.c
@@ -20,27 +20,24 @@
 
 /* This file implements the transport generic functions of the
  * ARM Debug Interface v5 Architecure Specification, ARM doc IHI0031A.
- *
- * Issues:
- * Currently doesn't use ROM table for introspection, just assumes
- * the device is Cortex-M3.
  */
 #include "general.h"
 #include "jtag_scan.h"
 #include "gdb_packet.h"
 #include "adiv5.h"
-#include "target.h"
+#include "cortexm.h"
 
 #ifndef DO_RESET_SEQ
 #define DO_RESET_SEQ 0
 #endif
 
-static const char adiv5_driver_str[] = "ARM ADIv5 MEM-AP";
+/* ROM table CIDR values */
+#define CIDR_ROM_TABLE  0xb105100d
+#define CIDR_GENERIC_IP 0xb105e00d
 
-static bool ap_check_error(target *t);
-
-static void ap_mem_read(target *t, void *dest, uint32_t src, size_t len);
-static void ap_mem_write(target *t, uint32_t dest, const void *src, size_t len);
+#define PIDR_REV_MASK 0x0FFF00000ULL
+#define PIDR_ARMv7M   0x4000BB000ULL
+#define PIDR_ARMv7MF  0x4000BB00CULL
 
 void adiv5_dp_ref(ADIv5_DP_t *dp)
 {
@@ -62,8 +59,6 @@ void adiv5_ap_unref(ADIv5_AP_t *ap)
 {
 	if (--(ap->refcnt) == 0) {
 		adiv5_dp_unref(ap->dp);
-		if (ap->priv)
-			ap->priv_free(ap->priv);
 		free(ap);
 	}
 }
@@ -73,6 +68,91 @@ void adiv5_dp_write(ADIv5_DP_t *dp, uint16_t addr, uint32_t value)
 	dp->low_access(dp, ADIV5_LOW_WRITE, addr, value);
 }
 
+static uint32_t adiv5_mem_read32(ADIv5_AP_t *ap, uint32_t addr)
+{
+	uint32_t ret;
+	adiv5_mem_read(ap, &ret, addr, sizeof(ret));
+	return ret;
+}
+
+static void adiv5_component_probe(ADIv5_AP_t *ap, uint32_t addr)
+{
+	addr &= ~3;
+	uint64_t pidr = 0;
+	uint32_t cidr = 0;
+	for (int i = 0; i < 4; i++) {
+		uint32_t x = adiv5_mem_read32(ap, addr + 0xfe0 + 4*i);
+		pidr |= (x & 0xff) << (i * 8);
+	}
+	pidr |= (uint64_t)adiv5_mem_read32(ap, addr + 0xfd0) << 32;
+	for (int i = 0; i < 4; i++) {
+		uint32_t x = adiv5_mem_read32(ap, addr + 0xff0 + 4*i);
+		cidr |= ((uint64_t)(x & 0xff)) << (i * 8);
+	}
+
+	switch (cidr) {
+	case CIDR_ROM_TABLE: /* This is a ROM table, probe recursively */
+		for (int i = 0; i < 256; i++) {
+			uint32_t entry = adiv5_mem_read32(ap, addr + i*4);
+			if (entry == 0)
+				break;
+
+			if ((entry & 1) == 0)
+				continue;
+
+			adiv5_component_probe(ap, addr + (entry & ~0xfff));
+		}
+		break;
+	case CIDR_GENERIC_IP:
+		switch (pidr & ~PIDR_REV_MASK) {
+		case PIDR_ARMv7MF:
+		case PIDR_ARMv7M:
+			cortexm_probe(ap);
+			break;
+		}
+		break;
+	}
+}
+
+ADIv5_AP_t *adiv5_new_ap(ADIv5_DP_t *dp, uint8_t apsel)
+{
+	ADIv5_AP_t *ap, tmpap;
+
+	/* Assume valid and try to read IDR */
+	memset(&tmpap, 0, sizeof(tmpap));
+	tmpap.dp = dp;
+	tmpap.apsel = apsel;
+	tmpap.idr = adiv5_ap_read(&tmpap, ADIV5_AP_IDR);
+
+	if(!tmpap.idr) /* IDR Invalid - Should we not continue here? */
+		return NULL;
+
+	/* Check for ARM Mem-AP */
+	uint16_t mfg = (tmpap.idr >> 17) & 0x3ff;
+	uint8_t cls = (tmpap.idr >> 13) & 0xf;
+	uint8_t type = tmpap.idr & 0xf;
+	if (mfg != 0x23B) /* Ditch if not ARM */
+		return NULL;
+	if ((cls != 8) || (type == 0)) /* Ditch if not Mem-AP */
+		return NULL;
+
+	/* It's valid to so create a heap copy */
+	ap = malloc(sizeof(*ap));
+	memcpy(ap, &tmpap, sizeof(*ap));
+	adiv5_dp_ref(dp);
+
+	ap->cfg = adiv5_ap_read(ap, ADIV5_AP_CFG);
+	ap->base = adiv5_ap_read(ap, ADIV5_AP_BASE);
+	ap->csw = adiv5_ap_read(ap, ADIV5_AP_CSW) &
+		~(ADIV5_AP_CSW_SIZE_MASK | ADIV5_AP_CSW_ADDRINC_MASK);
+
+	DEBUG("%3d: IDR=%08X CFG=%08X BASE=%08X CSW=%08X\n",
+	      apsel, ap->idr, ap->cfg, ap->base, ap->csw);
+
+	return ap;
+}
+
+
 void adiv5_dp_init(ADIv5_DP_t *dp)
 {
 	uint32_t ctrlstat;
@@ -113,56 +193,26 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 
 	/* Probe for APs on this DP */
 	for(int i = 0; i < 256; i++) {
-		ADIv5_AP_t *ap, tmpap;
-		target *t;
+		ADIv5_AP_t *ap = adiv5_new_ap(dp, i);
+		if (ap == NULL)
+			continue;
 
-		/* Assume valid and try to read IDR */
-		memset(&tmpap, 0, sizeof(tmpap));
-		tmpap.dp = dp;
-		tmpap.apsel = i;
-		tmpap.idr = adiv5_ap_read(&tmpap, ADIV5_AP_IDR);
-
-		if(!tmpap.idr) /* IDR Invalid - Should we not continue here? */
-			break;
-
-		/* It's valid to so create a heap copy */
-		ap = malloc(sizeof(*ap));
-		memcpy(ap, &tmpap, sizeof(*ap));
-		adiv5_dp_ref(dp);
-
-		ap->cfg = adiv5_ap_read(ap, ADIV5_AP_CFG);
-		ap->base = adiv5_ap_read(ap, ADIV5_AP_BASE);
-		ap->csw = adiv5_ap_read(ap, ADIV5_AP_CSW) &
-			~(ADIV5_AP_CSW_SIZE_MASK | ADIV5_AP_CSW_ADDRINC_MASK);
+		if (ap->base == 0xffffffff) {
+			/* No debug entries... useless AP */
+			adiv5_ap_unref(ap);
+			continue;
+		}
 
 		/* Should probe further here to make sure it's a valid target.
 		 * AP should be unref'd if not valid.
 		 */
 
-		/* Prepend to target list... */
-		t = target_new(sizeof(*t));
-		adiv5_ap_ref(ap);
-		t->priv = ap;
-		t->priv_free = (void (*)(void *))adiv5_ap_unref;
-
-		t->driver = adiv5_driver_str;
-		t->check_error = ap_check_error;
-
-		t->mem_read = ap_mem_read;
-		t->mem_write = ap_mem_write;
-
 		/* The rest sould only be added after checking ROM table */
-		cortexm_probe(t);
+		adiv5_component_probe(ap, ap->base);
 	}
 	adiv5_dp_unref(dp);
 }
 
-static bool ap_check_error(target *t)
-{
-	ADIv5_AP_t *ap = adiv5_target_ap(t);
-	return adiv5_dp_error(ap->dp) != 0;
-}
-
 enum align {
 	ALIGN_BYTE =  0,
 	ALIGN_HALFWORD = 1,
@@ -208,10 +258,9 @@ static void * extract(void *dest, uint32_t src, uint32_t val, enum align align)
 	return (uint8_t *)dest + (1 << align);
 }
 
-static void
-ap_mem_read(target *t, void *dest, uint32_t src, size_t len)
+void
+adiv5_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len)
 {
-	ADIv5_AP_t *ap = adiv5_target_ap(t);
 	uint32_t tmp;
 	uint32_t osrc = src;
 	enum align align = MIN(ALIGNOF(src), ALIGNOF(len));
@@ -237,10 +286,9 @@ ap_mem_read(target *t, void *dest, uint32_t src, size_t len)
 	extract(dest, src, tmp, align);
 }
 
-static void
-ap_mem_write(target *t, uint32_t dest, const void *src, size_t len)
+void
+adiv5_mem_write(ADIv5_AP_t *ap, uint32_t dest, const void *src, size_t len)
 {
-	ADIv5_AP_t *ap = adiv5_target_ap(t);
 	uint32_t odest = dest;
 	enum align align = MIN(ALIGNOF(dest), ALIGNOF(len));
 
diff --git a/src/cortexm.c b/src/cortexm.c
index 1d0fdfb2..5f9c9c11 100644
--- a/src/cortexm.c
+++ b/src/cortexm.c
@@ -82,6 +82,7 @@ static int cortexm_hostio_request(target *t);
 static void cortexm_hostio_reply(target *t, int32_t retcode, uint32_t errcode);
 
 struct cortexm_priv {
+	ADIv5_AP_t *ap;
 	bool stepping;
 	bool on_bkpt;
 	/* Watchpoint unit status */
@@ -197,8 +198,48 @@ static const char tdesc_cortex_mf[] =
 	"  </feature>"
 	"</target>";
 
-bool cortexm_probe(target *t)
+ADIv5_AP_t *cortexm_ap(target *t)
 {
+	return ((struct cortexm_priv *)t->priv)->ap;
+}
+
+static void cortexm_mem_read(target *t, void *dest, uint32_t src, size_t len)
+{
+	adiv5_mem_read(cortexm_ap(t), dest, src, len);
+}
+
+static void cortexm_mem_write(target *t, uint32_t dest, const void *src, size_t len)
+{
+	adiv5_mem_write(cortexm_ap(t), dest, src, len);
+}
+
+static bool cortexm_check_error(target *t)
+{
+	ADIv5_AP_t *ap = cortexm_ap(t);
+	return adiv5_dp_error(ap->dp) != 0;
+}
+
+static void cortexm_priv_free(void *priv)
+{
+	adiv5_ap_unref(((struct cortexm_priv *)priv)->ap);
+	free(priv);
+}
+
+bool cortexm_probe(ADIv5_AP_t *ap)
+{
+	target *t;
+
+	t = target_new(sizeof(*t));
+	adiv5_ap_ref(ap);
+	struct cortexm_priv *priv = calloc(1, sizeof(*priv));
+	t->priv = priv;
+	t->priv_free = cortexm_priv_free;
+	priv->ap = ap;
+
+	t->check_error = cortexm_check_error;
+	t->mem_read = cortexm_mem_read;
+	t->mem_write = cortexm_mem_write;
+
 	t->driver = cortexm_driver_str;
 
 	t->attach = cortexm_attach;
@@ -229,11 +270,6 @@ bool cortexm_probe(target *t)
 		t->tdesc = tdesc_cortex_mf;
 	}
 
-	ADIv5_AP_t *ap = adiv5_target_ap(t);
-	struct cortexm_priv *priv = calloc(1, sizeof(*priv));
-	ap->priv = priv;
-	ap->priv_free = free;
-
 	/* Default vectors to catch */
 	priv->demcr = CORTEXM_DEMCR_TRCENA | CORTEXM_DEMCR_VC_HARDERR |
 			CORTEXM_DEMCR_VC_CORERESET;
@@ -260,8 +296,7 @@ bool cortexm_probe(target *t)
 
 bool cortexm_attach(target *t)
 {
-	ADIv5_AP_t *ap = adiv5_target_ap(t);
-	struct cortexm_priv *priv = ap->priv;
+	struct cortexm_priv *priv = t->priv;
 	unsigned i;
 	uint32_t r;
 	int tries;
@@ -323,8 +358,7 @@ bool cortexm_attach(target *t)
 
 void cortexm_detach(target *t)
 {
-	ADIv5_AP_t *ap = adiv5_target_ap(t);
-	struct cortexm_priv *priv = ap->priv;
+	struct cortexm_priv *priv = t->priv;
 	unsigned i;
 
 	/* Clear any stale breakpoints */
@@ -343,7 +377,7 @@ enum { DB_DHCSR, DB_DCRSR, DB_DCRDR, DB_DEMCR };
 
 static void cortexm_regs_read(target *t, void *data)
 {
-	ADIv5_AP_t *ap = adiv5_target_ap(t);
+	ADIv5_AP_t *ap = cortexm_ap(t);
 	uint32_t *regs = data;
 	unsigned i;
 
@@ -374,7 +408,7 @@ static void cortexm_regs_read(target *t, void *data)
 
 static void cortexm_regs_write(target *t, const void *data)
 {
-	ADIv5_AP_t *ap = adiv5_target_ap(t);
+	ADIv5_AP_t *ap = cortexm_ap(t);
 	const uint32_t *regs = data;
 	unsigned i;
 
@@ -459,8 +493,7 @@ static void cortexm_halt_request(target *t)
 
 static int cortexm_halt_wait(target *t)
 {
-	ADIv5_AP_t *ap = adiv5_target_ap(t);
-	struct cortexm_priv *priv = ap->priv;
+	struct cortexm_priv *priv = t->priv;
 
 	volatile uint32_t dhcsr = 0;
 	volatile struct exception e;
@@ -521,8 +554,7 @@ static int cortexm_halt_wait(target *t)
 
 void cortexm_halt_resume(target *t, bool step)
 {
-	ADIv5_AP_t *ap = adiv5_target_ap(t);
-	struct cortexm_priv *priv = ap->priv;
+	struct cortexm_priv *priv = t->priv;
 	uint32_t dhcsr = CORTEXM_DHCSR_DBGKEY | CORTEXM_DHCSR_C_DEBUGEN;
 
 	if (step)
@@ -639,8 +671,7 @@ int cortexm_run_stub(target *t, uint32_t loadaddr,
 
 static int cortexm_set_hw_bp(target *t, uint32_t addr)
 {
-	ADIv5_AP_t *ap = adiv5_target_ap(t);
-	struct cortexm_priv *priv = ap->priv;
+	struct cortexm_priv *priv = t->priv;
 	uint32_t val = addr;
 	unsigned i;
 
@@ -664,8 +695,7 @@ static int cortexm_set_hw_bp(target *t, uint32_t addr)
 
 static int cortexm_clear_hw_bp(target *t, uint32_t addr)
 {
-	ADIv5_AP_t *ap = adiv5_target_ap(t);
-	struct cortexm_priv *priv = ap->priv;
+	struct cortexm_priv *priv = t->priv;
 	unsigned i;
 
 	for(i = 0; i < priv->hw_breakpoint_max; i++)
@@ -686,8 +716,7 @@ static int cortexm_clear_hw_bp(target *t, uint32_t addr)
 static int
 cortexm_set_hw_wp(target *t, uint8_t type, uint32_t addr, uint8_t len)
 {
-	ADIv5_AP_t *ap = adiv5_target_ap(t);
-	struct cortexm_priv *priv = ap->priv;
+	struct cortexm_priv *priv = t->priv;
 	unsigned i;
 
 	switch(len) { /* Convert bytes size to mask size */
@@ -728,8 +757,7 @@ cortexm_set_hw_wp(target *t, uint8_t type, uint32_t addr, uint8_t len)
 static int
 cortexm_clear_hw_wp(target *t, uint8_t type, uint32_t addr, uint8_t len)
 {
-	ADIv5_AP_t *ap = adiv5_target_ap(t);
-	struct cortexm_priv *priv = ap->priv;
+	struct cortexm_priv *priv = t->priv;
 	unsigned i;
 
 	switch(len) {
@@ -764,8 +792,7 @@ cortexm_clear_hw_wp(target *t, uint8_t type, uint32_t addr, uint8_t len)
 
 static int cortexm_check_hw_wp(target *t, uint32_t *addr)
 {
-	ADIv5_AP_t *ap = adiv5_target_ap(t);
-	struct cortexm_priv *priv = ap->priv;
+	struct cortexm_priv *priv = t->priv;
 	unsigned i;
 
 	for(i = 0; i < priv->hw_watchpoint_max; i++)
@@ -783,8 +810,7 @@ static int cortexm_check_hw_wp(target *t, uint32_t *addr)
 
 static bool cortexm_vector_catch(target *t, int argc, char *argv[])
 {
-	ADIv5_AP_t *ap = adiv5_target_ap(t);
-	struct cortexm_priv *priv = ap->priv;
+	struct cortexm_priv *priv = t->priv;
 	const char *vectors[] = {"reset", NULL, NULL, NULL, "mm", "nocp",
 				"chk", "stat", "bus", "int", "hard"};
 	uint32_t tmp = 0;
@@ -862,8 +888,7 @@ static bool cortexm_vector_catch(target *t, int argc, char *argv[])
 
 static int cortexm_hostio_request(target *t)
 {
-	ADIv5_AP_t *ap = adiv5_target_ap(t);
-	struct cortexm_priv *priv = ap->priv;
+	struct cortexm_priv *priv = t->priv;
 	uint32_t arm_regs[t->regs_size];
 	uint32_t params[4];
 
@@ -964,8 +989,7 @@ static int cortexm_hostio_request(target *t)
 
 static void cortexm_hostio_reply(target *t, int32_t retcode, uint32_t errcode)
 {
-	ADIv5_AP_t *ap = adiv5_target_ap(t);
-	struct cortexm_priv *priv = ap->priv;
+	struct cortexm_priv *priv = t->priv;
 	uint32_t arm_regs[t->regs_size];
 
 	DEBUG("syscall return ret=%d errno=%d\n", retcode, errcode);
diff --git a/src/efm32.c b/src/efm32.c
index 25bb3f28..52b2ea0c 100644
--- a/src/efm32.c
+++ b/src/efm32.c
@@ -252,7 +252,7 @@ char variant_string[40];
 bool efm32_probe(target *t)
 {
 	/* Read the IDCODE register from the SW-DP */
-	ADIv5_AP_t *ap = adiv5_target_ap(t);
+	ADIv5_AP_t *ap = cortexm_ap(t);
 	uint32_t ap_idcode = ap->dp->idcode;
 
 	/* Check the idcode is silabs. See AN0062 Section 2.2 */
diff --git a/src/include/adiv5.h b/src/include/adiv5.h
index 12d3bf4a..b6fe30f6 100644
--- a/src/include/adiv5.h
+++ b/src/include/adiv5.h
@@ -22,7 +22,6 @@
 #define __ADIV5_H
 
 #include "jtag_scan.h"
-#include "target.h"
 
 #define ADIV5_APnDP       0x100
 #define ADIV5_DP_REG(x)   (x)
@@ -144,14 +143,12 @@ typedef struct ADIv5_AP_s {
 	uint32_t cfg;
 	uint32_t base;
 	uint32_t csw;
-
-	void *priv;
-	void (*priv_free)(void *);
 } ADIv5_AP_t;
 
 void adiv5_dp_init(ADIv5_DP_t *dp);
 void adiv5_dp_write(ADIv5_DP_t *dp, uint16_t addr, uint32_t value);
 
+ADIv5_AP_t *adiv5_new_ap(ADIv5_DP_t *dp, uint8_t apsel);
 void adiv5_dp_ref(ADIv5_DP_t *dp);
 void adiv5_ap_ref(ADIv5_AP_t *ap);
 void adiv5_dp_unref(ADIv5_DP_t *dp);
@@ -163,10 +160,8 @@ uint32_t adiv5_ap_read(ADIv5_AP_t *ap, uint16_t addr);
 void adiv5_jtag_dp_handler(jtag_dev_t *dev);
 int adiv5_swdp_scan(void);
 
-static inline ADIv5_AP_t *adiv5_target_ap(target *target)
-{
-	return target->priv;
-}
+void adiv5_mem_read(ADIv5_AP_t *ap, void *dest, uint32_t src, size_t len);
+void adiv5_mem_write(ADIv5_AP_t *ap, uint32_t dest, const void *src, size_t len);
 
 #endif
 
diff --git a/src/include/cortexm.h b/src/include/cortexm.h
index de571123..bf1d821d 100644
--- a/src/include/cortexm.h
+++ b/src/include/cortexm.h
@@ -20,6 +20,7 @@
 #define __CORTEXM_H
 
 #include "target.h"
+#include "adiv5.h"
 
 /* Private peripheral bus base address */
 #define CORTEXM_PPB_BASE	0xE0000000
@@ -155,6 +156,9 @@
 
 #define	CORTEXM_TOPT_INHIBIT_SRST (1 << 2)
 
+bool cortexm_probe(ADIv5_AP_t *ap);
+ADIv5_AP_t *cortexm_ap(target *t);
+
 bool cortexm_attach(target *t);
 void cortexm_detach(target *t);
 void cortexm_halt_resume(target *t, bool step);
diff --git a/src/include/target.h b/src/include/target.h
index 1a4138fe..9e732790 100644
--- a/src/include/target.h
+++ b/src/include/target.h
@@ -257,7 +257,6 @@ static inline void target_mem_write8(target *t, uint32_t addr, uint8_t value)
 /* Probe for various targets.
  * Actual functions implemented in their respective drivers.
  */
-bool cortexm_probe(target *t);
 bool stm32f1_probe(target *t);
 bool stm32f4_probe(target *t);
 bool stm32l0_probe(target *t);
diff --git a/src/jtag_scan.c b/src/jtag_scan.c
index 2d2194c7..3bd3be1b 100644
--- a/src/jtag_scan.c
+++ b/src/jtag_scan.c
@@ -28,6 +28,7 @@
 #include "morse.h"
 #include "jtag_scan.h"
 #include "gdb_packet.h"
+#include "target.h"
 #include "adiv5.h"
 #include "arm7tdmi.h"