1 files changed, 61 insertions, 47 deletions

diff --git a/src/flash/nor/at91samd.c b/src/flash/nor/at91samd.c
index 58b367a..f018e89 100644
--- a/src/flash/nor/at91samd.c
+++ b/src/flash/nor/at91samd.c
@@ -36,6 +36,7 @@
 
 #define SAMD_DSU_STATUSA        1               /* DSU status register */
 #define SAMD_DSU_DID		0x18		/* Device ID register */
+#define SAMD_DSU_CTRL_EXT	0x100		/* CTRL register, external access */
 
 #define SAMD_NVMCTRL_CTRLA		0x00	/* NVM control A register */
 #define SAMD_NVMCTRL_CTRLB		0x04	/* NVM control B register */
@@ -423,39 +424,43 @@ static int samd_probe(struct flash_bank *bank)
 	return ERROR_OK;
 }
 
-static bool samd_check_error(struct target *target)
+static int samd_check_error(struct target *target)
 {
-	int ret;
-	bool error;
+	int ret, ret2;
 	uint16_t status;
 
 	ret = target_read_u16(target,
 			SAMD_NVMCTRL + SAMD_NVMCTRL_STATUS, &status);
 	if (ret != ERROR_OK) {
 		LOG_ERROR("Can't read NVM status");
-		return true;
+		return ret;
 	}
 
-	if (status & 0x001C) {
-		if (status & (1 << 4)) /* NVME */
-			LOG_ERROR("SAMD: NVM Error");
-		if (status & (1 << 3)) /* LOCKE */
-			LOG_ERROR("SAMD: NVM lock error");
-		if (status & (1 << 2)) /* PROGE */
-			LOG_ERROR("SAMD: NVM programming error");
+	if ((status & 0x001C) == 0)
+		return ERROR_OK;
 
-		error = true;
-	} else {
-		error = false;
+	if (status & (1 << 4)) { /* NVME */
+		LOG_ERROR("SAMD: NVM Error");
+		ret = ERROR_FLASH_OPERATION_FAILED;
+	}
+
+	if (status & (1 << 3)) { /* LOCKE */
+		LOG_ERROR("SAMD: NVM lock error");
+		ret = ERROR_FLASH_PROTECTED;
+	}
+
+	if (status & (1 << 2)) { /* PROGE */
+		LOG_ERROR("SAMD: NVM programming error");
+		ret = ERROR_FLASH_OPER_UNSUPPORTED;
 	}
 
 	/* Clear the error conditions by writing a one to them */
-	ret = target_write_u16(target,
+	ret2 = target_write_u16(target,
 			SAMD_NVMCTRL + SAMD_NVMCTRL_STATUS, status);
-	if (ret != ERROR_OK)
+	if (ret2 != ERROR_OK)
 		LOG_ERROR("Can't clear NVM error conditions");
 
-	return error;
+	return ret;
 }
 
 static int samd_issue_nvmctrl_command(struct target *target, uint16_t cmd)
@@ -474,10 +479,7 @@ static int samd_issue_nvmctrl_command(struct target *target, uint16_t cmd)
 		return res;
 
 	/* Check to see if the NVM command resulted in an error condition. */
-	if (samd_check_error(target))
-		return ERROR_FAIL;
-
-	return ERROR_OK;
+	return samd_check_error(target);
 }
 
 static int samd_erase_row(struct target *target, uint32_t address)
@@ -531,12 +533,19 @@ static int samd_modify_user_row(struct target *target, uint32_t value,
 		uint8_t startb, uint8_t endb)
 {
 	int res;
+	uint32_t nvm_ctrlb;
+	bool manual_wp = true;
 
 	if (is_user_row_reserved_bit(startb) || is_user_row_reserved_bit(endb)) {
 		LOG_ERROR("Can't modify bits in the requested range");
 		return ERROR_FAIL;
 	}
 
+	/* Check if we need to do manual page write commands */
+	res = target_read_u32(target, SAMD_NVMCTRL + SAMD_NVMCTRL_CTRLB, &nvm_ctrlb);
+	if (res == ERROR_OK)
+		manual_wp = (nvm_ctrlb & SAMD_NVM_CTRLB_MANW) != 0;
+
 	/* Retrieve the MCU's page size, in bytes. This is also the size of the
 	 * entire User Row. */
 	uint32_t page_size;
@@ -559,8 +568,8 @@ static int samd_modify_user_row(struct target *target, uint32_t value,
 	if (!buf)
 		return ERROR_FAIL;
 
-	/* Read the user row (comprising one page) by half-words. */
-	res = target_read_memory(target, SAMD_USER_ROW, 2, page_size / 2, buf);
+	/* Read the user row (comprising one page) by words. */
+	res = target_read_memory(target, SAMD_USER_ROW, 4, page_size / 4, buf);
 	if (res != ERROR_OK)
 		goto out_user_row;
 
@@ -579,20 +588,18 @@ static int samd_modify_user_row(struct target *target, uint32_t value,
 	/* Modify */
 	buf_set_u32(buf, startb, endb - startb + 1, value);
 
-	/* Write the page buffer back out to the target.  A Flash write will be
-	 * triggered automatically. */
+	/* Write the page buffer back out to the target. */
 	res = target_write_memory(target, SAMD_USER_ROW, 4, page_size / 4, buf);
 	if (res != ERROR_OK)
 		goto out_user_row;
 
-	if (samd_check_error(target)) {
-		res = ERROR_FAIL;
-		goto out_user_row;
+	if (manual_wp) {
+		/* Trigger flash write */
+		res = samd_issue_nvmctrl_command(target, SAMD_NVM_CMD_WAP);
+	} else {
+		res = samd_check_error(target);
 	}
 
-	/* Success */
-	res = ERROR_OK;
-
 out_user_row:
 	free(buf);
 
@@ -784,18 +791,15 @@ static int samd_write(struct flash_bank *bank, const uint8_t *buffer,
 		 * then issue CMD_WP always */
 		if (manual_wp || pg_offset + 4 * nw < chip->page_size) {
 			res = samd_issue_nvmctrl_command(bank->target, SAMD_NVM_CMD_WP);
-			if (res != ERROR_OK) {
-				LOG_ERROR("%s: %d", __func__, __LINE__);
-				goto free_pb;
-			}
-		}
+		} else {
+			/* Access through AHB is stalled while flash is being programmed */
+			usleep(200);
 
-		/* Access through AHB is stalled while flash is being programmed */
-		usleep(200);
+			res = samd_check_error(bank->target);
+		}
 
-		if (samd_check_error(bank->target)) {
+		if (res != ERROR_OK) {
 			LOG_ERROR("%s: write failed at address 0x%08" PRIx32, __func__, address);
-			res = ERROR_FAIL;
 			goto free_pb;
 		}
 
@@ -856,18 +860,23 @@ COMMAND_HANDLER(samd_handle_info_command)
 COMMAND_HANDLER(samd_handle_chip_erase_command)
 {
 	struct target *target = get_current_target(CMD_CTX);
+	int res = ERROR_FAIL;
 
 	if (target) {
 		/* Enable access to the DSU by disabling the write protect bit */
 		target_write_u32(target, SAMD_PAC1, (1<<1));
+		/* intentionally without error checking - not accessible on secured chip */
+
 		/* Tell the DSU to perform a full chip erase.  It takes about 240ms to
 		 * perform the erase. */
-		target_write_u8(target, SAMD_DSU, (1<<4));
-
-		command_print(CMD_CTX, "chip erased");
+		res = target_write_u8(target, SAMD_DSU + SAMD_DSU_CTRL_EXT, (1<<4));
+		if (res == ERROR_OK)
+			command_print(CMD_CTX, "chip erase started");
+		else
+			command_print(CMD_CTX, "write to DSU CTRL failed");
 	}
 
-	return ERROR_OK;
+	return res;
 }
 
 COMMAND_HANDLER(samd_handle_set_security_command)
@@ -1018,10 +1027,15 @@ COMMAND_HANDLER(samd_handle_bootloader_command)
 COMMAND_HANDLER(samd_handle_reset_deassert)
 {
 	struct target *target = get_current_target(CMD_CTX);
-	struct armv7m_common *armv7m = target_to_armv7m(target);
 	int retval = ERROR_OK;
 	enum reset_types jtag_reset_config = jtag_get_reset_config();
 
+	/* If the target has been unresponsive before, try to re-establish
+	 * communication now - CPU is held in reset by DSU, DAP is working */
+	if (!target_was_examined(target))
+		target_examine_one(target);
+	target_poll(target);
+
 	/* In case of sysresetreq, debug retains state set in cortex_m_assert_reset()
 	 * so we just release reset held by DSU
 	 *
@@ -1030,9 +1044,9 @@ COMMAND_HANDLER(samd_handle_reset_deassert)
 	 * After vectreset DSU release is not needed however makes no harm
 	 */
 	if (target->reset_halt && (jtag_reset_config & RESET_HAS_SRST)) {
-		retval = mem_ap_write_u32(armv7m->debug_ap, DCB_DHCSR, DBGKEY | C_HALT | C_DEBUGEN);
+		retval = target_write_u32(target, DCB_DHCSR, DBGKEY | C_HALT | C_DEBUGEN);
 		if (retval == ERROR_OK)
-			retval = mem_ap_write_u32(armv7m->debug_ap, DCB_DEMCR,
+			retval = target_write_u32(target, DCB_DEMCR,
 				TRCENA | VC_HARDERR | VC_BUSERR | VC_CORERESET);
 		/* do not return on error here, releasing DSU reset is more important */
 	}