From 7de304d72aedad6b03ffa2ecf3b9c12ffe432900 Mon Sep 17 00:00:00 2001
From: Gareth McMullin <gareth@blacksphere.co.nz>
Date: Mon, 27 Mar 2017 10:51:36 +1300
Subject: [PATCH] kinetis: Add recovery mode target.

---
 src/target/adiv5.c   |  12 ++---
 src/target/kinetis.c | 110 +++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 113 insertions(+), 9 deletions(-)

diff --git a/src/target/adiv5.c b/src/target/adiv5.c
index ed30f69..8d86936 100644
--- a/src/target/adiv5.c
+++ b/src/target/adiv5.c
@@ -364,15 +364,6 @@ ADIv5_AP_t *adiv5_new_ap(ADIv5_DP_t *dp, uint8_t apsel)
 	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));
@@ -447,6 +438,9 @@ void adiv5_dp_init(ADIv5_DP_t *dp)
 		if (ap == NULL)
 			continue;
 
+		extern void kinetis_mdm_probe(ADIv5_AP_t *);
+		kinetis_mdm_probe(ap);
+
 		if (ap->base == 0xffffffff) {
 			/* No debug entries... useless AP */
 			adiv5_ap_unref(ap);
diff --git a/src/target/kinetis.c b/src/target/kinetis.c
index 36bbefb..af0326d 100644
--- a/src/target/kinetis.c
+++ b/src/target/kinetis.c
@@ -199,3 +199,113 @@ static int kl_gen_flash_write(struct target_flash *f,
 	}
 	return 0;
 }
+
+/*** Kinetis recovery mode using the MDM-AP ***/
+
+/* Kinetis security bits are stored in regular flash, so it is possible
+ * to enable protection by accident when flashing a bad binary.
+ * a backdoor AP is provided which may allow a mass erase to recover the
+ * device.  This provides a fake target to allow a monitor command interface
+ */
+#include "adiv5.h"
+
+#define KINETIS_MDM_IDR_K22F 0x1c0000
+#define KINETIS_MDM_IDR_KZ03 0x1c0020
+
+static bool kinetis_mdm_cmd_erase_mass(target *t);
+
+const struct command_s kinetis_mdm_cmd_list[] = {
+	{"erase_mass", (cmd_handler)kinetis_mdm_cmd_erase_mass, "Erase entire flash memory"},
+	{NULL, NULL, NULL}
+};
+
+bool nop_function(void)
+{
+	return true;
+}
+
+enum target_halt_reason mdm_halt_poll(target *t, target_addr *watch)
+{
+	(void)t; (void)watch;
+	return TARGET_HALT_REQUEST;
+}
+
+void kinetis_mdm_probe(ADIv5_AP_t *ap)
+{
+	switch(ap->idr) {
+	case KINETIS_MDM_IDR_KZ03:
+	case KINETIS_MDM_IDR_K22F:
+		break;
+	default:
+		return;
+	}
+
+	target *t = target_new();
+	adiv5_ap_ref(ap);
+	t->priv = ap;
+	t->priv_free = (void*)adiv5_ap_unref;
+
+	t->driver = "Kinetis Recovery (MDM-AP)";
+	t->attach = (void*)nop_function;
+	t->detach = (void*)nop_function;
+	t->check_error = (void*)nop_function;
+	t->mem_read = (void*)nop_function;
+	t->mem_write = (void*)nop_function;
+	t->regs_size = 4;
+	t->regs_read = (void*)nop_function;
+	t->regs_write = (void*)nop_function;
+	t->reset = (void*)nop_function;
+	t->halt_request = (void*)nop_function;
+	t->halt_poll = mdm_halt_poll;
+	t->halt_resume = (void*)nop_function;
+
+	target_add_commands(t, kinetis_mdm_cmd_list, t->driver);
+}
+
+#define MDM_STATUS  ADIV5_AP_REG(0x00)
+#define MDM_CONTROL ADIV5_AP_REG(0x04)
+
+#define MDM_STATUS_MASS_ERASE_ACK (1 << 0)
+#define MDM_STATUS_FLASH_READY (1 << 1)
+#define MDM_STATUS_MASS_ERASE_ENABLED (1 << 5)
+
+#define MDM_CONTROL_MASS_ERASE (1 << 0)
+
+static bool kinetis_mdm_cmd_erase_mass(target *t)
+{
+	ADIv5_AP_t *ap = t->priv;
+
+	uint32_t status, control;
+	status = adiv5_ap_read(ap, MDM_STATUS);
+	control = adiv5_ap_read(ap, MDM_CONTROL);
+	tc_printf(t, "Requesting mass erase (status = 0x%"PRIx32")\n", status);
+
+	if (!(status & MDM_STATUS_MASS_ERASE_ENABLED)) {
+		tc_printf(t, "ERROR: Mass erase disabled!\n");
+		return false;
+	}
+
+	if (!(status & MDM_STATUS_FLASH_READY)) {
+		tc_printf(t, "ERROR: Flash not ready!\n");
+		return false;
+	}
+
+	if (status & MDM_STATUS_MASS_ERASE_ACK) {
+		tc_printf(t, "ERROR: Mass erase already in progress!\n");
+		return false;
+	}
+
+	adiv5_ap_write(ap, MDM_CONTROL, MDM_CONTROL_MASS_ERASE);
+
+	do {
+		status = adiv5_ap_read(ap, MDM_STATUS);
+	} while (!(status & MDM_STATUS_MASS_ERASE_ACK));
+	tc_printf(t, "Mass erase acknowledged\n");
+
+	do {
+		control = adiv5_ap_read(ap, MDM_CONTROL);
+	} while (!(control & MDM_CONTROL_MASS_ERASE));
+	tc_printf(t, "Mass erase complete\n");
+
+	return true;
+}