diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index f8a2e2278..21dc54cd3 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -955,6 +955,186 @@ int mem_ap_sel_write_buf_u32(struct adiv5_dap *swjdp, uint8_t ap,
 	return mem_ap_write_buf_u32(swjdp, buffer, count, address);
 }
 
+#define MDM_REG_STAT		0x00
+#define MDM_REG_CTRL		0x04
+#define MDM_REG_ID		0xfc
+
+#define MDM_STAT_FMEACK		(1<<0)
+#define MDM_STAT_FREADY		(1<<1)
+#define MDM_STAT_SYSSEC		(1<<2)
+#define MDM_STAT_SYSRES		(1<<3)
+#define MDM_STAT_FMEEN		(1<<5)
+#define MDM_STAT_BACKDOOREN	(1<<6)
+#define MDM_STAT_LPEN		(1<<7)
+#define MDM_STAT_VLPEN		(1<<8)
+#define MDM_STAT_LLSMODEXIT	(1<<9)
+#define MDM_STAT_VLLSXMODEXIT	(1<<10)
+#define MDM_STAT_CORE_HALTED	(1<<16)
+#define MDM_STAT_CORE_SLEEPDEEP	(1<<17)
+#define MDM_STAT_CORESLEEPING	(1<<18)
+
+#define MEM_CTRL_FMEIP		(1<<0)
+#define MEM_CTRL_DBG_DIS	(1<<1)
+#define MEM_CTRL_DBG_REQ	(1<<2)
+#define MEM_CTRL_SYS_RES_REQ	(1<<3)
+#define MEM_CTRL_CORE_HOLD_RES	(1<<4)
+#define MEM_CTRL_VLLSX_DBG_REQ	(1<<5)
+#define MEM_CTRL_VLLSX_DBG_ACK	(1<<6)
+#define MEM_CTRL_VLLSX_STAT_ACK	(1<<7)
+
+/**
+ *
+ */
+int dap_syssec_kinetis_mdmap(struct adiv5_dap *dap)
+{
+	uint32_t val;
+	int retval;
+	enum reset_types jtag_reset_config = jtag_get_reset_config();
+
+	dap_ap_select(dap, 1);
+
+	/* first check mdm-ap id register */
+	retval = dap_queue_ap_read(dap, MDM_REG_ID, &val);
+	if (retval != ERROR_OK)
+		return retval;
+	dap_run(dap);
+
+	if ( val != 0x001C0000 )
+	{
+		LOG_DEBUG("id doesn't match %08X != 0x001C0000",val);
+		dap_ap_select(dap, 0);
+		return ERROR_FAIL;
+	}
+
+	/* read and parse status register
+	 * it's important that the device is out of
+	 * reset here
+	 */
+	retval = dap_queue_ap_read(dap, MDM_REG_STAT, &val);
+	if (retval != ERROR_OK)
+		return retval;
+	dap_run(dap);
+
+	LOG_DEBUG("MDM_REG_STAT %08X",val);
+
+	if ( (val & (MDM_STAT_SYSSEC|MDM_STAT_FREADY)) != (MDM_STAT_FREADY) )
+	{
+		LOG_DEBUG("MDMAP: system is secured, masserase needed");
+
+		if ( !(val & MDM_STAT_FMEEN) )
+		{
+			LOG_DEBUG("MDMAP: masserase is disabled");
+		}
+		else
+		{
+			/* we need to assert reset */
+			if ( jtag_reset_config & RESET_HAS_SRST )
+			{
+				/* default to asserting srst */
+				if (jtag_reset_config & RESET_SRST_PULLS_TRST)
+				{
+					jtag_add_reset(1, 1);
+				}
+				else
+				{
+					jtag_add_reset(0, 1);
+				}
+			}
+			else
+			{
+				LOG_DEBUG("SRST not configured");
+				dap_ap_select(dap, 0);
+				return ERROR_FAIL;
+			}
+
+			while(1)
+			{
+				retval = dap_queue_ap_write(dap, MDM_REG_CTRL, MEM_CTRL_FMEIP);
+				if (retval != ERROR_OK)
+					return retval;
+				dap_run(dap);
+				/* read status register and wait for ready */
+				retval = dap_queue_ap_read(dap, MDM_REG_STAT, &val);
+				if (retval != ERROR_OK)
+					return retval;
+				dap_run(dap);
+				LOG_DEBUG("MDM_REG_STAT %08X",val);
+
+				if ( (val&1))
+					break;
+			}
+
+			while(1)
+			{
+				retval = dap_queue_ap_write(dap, MDM_REG_CTRL, 0);
+				if (retval != ERROR_OK)
+					return retval;
+				dap_run(dap);
+				/* read status register */
+				retval = dap_queue_ap_read(dap, MDM_REG_STAT, &val);
+				if (retval != ERROR_OK)
+					return retval;
+				dap_run(dap);
+				LOG_DEBUG("MDM_REG_STAT %08X",val);
+				/* read control register and wait for ready */
+				retval = dap_queue_ap_read(dap, MDM_REG_CTRL, &val);
+				if (retval != ERROR_OK)
+					return retval;
+				dap_run(dap);
+				LOG_DEBUG("MDM_REG_CTRL %08X",val);
+
+				if ( val == 0x00 )
+					break;
+			}
+		}
+	}
+
+	dap_ap_select(dap, 0);
+
+	return ERROR_OK;
+}
+
+/** */
+struct dap_syssec_filter {
+	/** */
+	uint32_t idcode;
+	/** */
+	int (*dap_init)(struct adiv5_dap *dap);
+};
+
+/** */
+static struct dap_syssec_filter dap_syssec_filter_data[] = {
+	{ 0x4BA00477, dap_syssec_kinetis_mdmap }
+};
+
+
+/**
+ *
+ */
+int dap_syssec(struct adiv5_dap *dap)
+{
+	unsigned int i;
+	struct jtag_tap *tap;
+
+	for(i=0;i<sizeof(dap_syssec_filter_data);i++)
+	{
+		tap = dap->jtag_info->tap;
+
+		while (tap != NULL)
+		{
+			if (!tap->hasidcode)
+				continue;
+			if ( dap_syssec_filter_data[i].idcode == tap->idcode )
+			{
+				LOG_DEBUG("DAP: mdmap_init for idcode: %08x",tap->idcode);
+				dap_syssec_filter_data[i].dap_init(dap);
+			}
+			tap = tap->next_tap;
+		}
+	}
+
+	return ERROR_OK;
+}
 
 /*--------------------------------------------------------------------------*/
 
@@ -1062,6 +1242,8 @@ int ahbap_debugport_init(struct adiv5_dap *dap)
 	if (retval != ERROR_OK)
 		return retval;
 
+	dap_syssec(dap);
+
 	return ERROR_OK;
 }