flash/stm32l4x: introduce 'stm32l4x trustzone [enable|disable]' command

this command will help to enable/disable or display the TrustZone security,
using the TZEN option bit.

Note: This command works only with devices with TrustZone, eg. STM32L5.
Note: This command will perform an OBL_Launch after modifying the TZEN.

Change-Id: I4aef15bf57d09c1658d37858143d23b1d43de1f0
Signed-off-by: Tarek BOCHKATI <tarek.bouchkati@gmail.com>
Reviewed-on: https://review.openocd.org/c/openocd/+/5542
Tested-by: jenkins
Reviewed-by: Antonio Borneo <borneo.antonio@gmail.com>
Reviewed-by: Oleksij Rempel <linux@rempel-privat.de>

1 files changed, 108 insertions, 15 deletions

diff --git a/src/flash/nor/stm32l4x.c b/src/flash/nor/stm32l4x.c
index 5287ff6..6370d17 100644
--- a/src/flash/nor/stm32l4x.c
+++ b/src/flash/nor/stm32l4x.c
@@ -791,6 +791,42 @@ static int stm32l4_unlock_option_reg(struct flash_bank *bank)
 	return ERROR_OK;
 }
 
+static int stm32l4_perform_obl_launch(struct flash_bank *bank)
+{
+	int retval, retval2;
+
+	retval = stm32l4_unlock_reg(bank);
+	if (retval != ERROR_OK)
+		goto err_lock;
+
+	retval = stm32l4_unlock_option_reg(bank);
+	if (retval != ERROR_OK)
+		goto err_lock;
+
+	/* Set OBL_LAUNCH bit in CR -> system reset and option bytes reload,
+	 * but the RMs explicitly do *NOT* list this as power-on reset cause, and:
+	 * "Note: If the read protection is set while the debugger is still
+	 * connected through JTAG/SWD, apply a POR (power-on reset) instead of a system reset."
+	 */
+
+	/* "Setting OBL_LAUNCH generates a reset so the option byte loading is performed under system reset" */
+	/* Due to this reset ST-Link reports an SWD_DP_ERROR, despite the write was successful,
+	 * then just ignore the returned value */
+	stm32l4_write_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, FLASH_OBL_LAUNCH);
+
+	/* Need to re-probe after change */
+	struct stm32l4_flash_bank *stm32l4_info = bank->driver_priv;
+	stm32l4_info->probed = false;
+
+err_lock:
+	retval2 = stm32l4_write_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, FLASH_LOCK | FLASH_OPTLOCK);
+
+	if (retval != ERROR_OK)
+		return retval;
+
+	return retval2;
+}
+
 static int stm32l4_write_option(struct flash_bank *bank, uint32_t reg_offset,
 	uint32_t value, uint32_t mask)
 {
@@ -1775,9 +1811,9 @@ COMMAND_HANDLER(stm32l4_handle_option_write_command)
 	return retval;
 }
 
-COMMAND_HANDLER(stm32l4_handle_option_load_command)
+COMMAND_HANDLER(stm32l4_handle_trustzone_command)
 {
-	if (CMD_ARGC != 1)
+	if (CMD_ARGC < 1 || CMD_ARGC > 2)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
 	struct flash_bank *bank;
@@ -1785,28 +1821,78 @@ COMMAND_HANDLER(stm32l4_handle_option_load_command)
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = stm32l4_unlock_reg(bank);
+	struct stm32l4_flash_bank *stm32l4_info = bank->driver_priv;
+	if (!(stm32l4_info->part_info->flags & F_HAS_TZ)) {
+		LOG_ERROR("This device does not have a TrustZone");
+		return ERROR_FAIL;
+	}
+
+	uint32_t optr;
+	retval = stm32l4_read_flash_reg_by_index(bank, STM32_FLASH_OPTR_INDEX, &optr);
 	if (retval != ERROR_OK)
 		return retval;
 
-	retval = stm32l4_unlock_option_reg(bank);
+	stm32l4_sync_rdp_tzen(bank, optr);
+
+	if (CMD_ARGC == 1) {
+		/* only display the TZEN value */
+		LOG_INFO("Global TrustZone Security is %s", stm32l4_info->tzen ? "enabled" : "disabled");
+		return ERROR_OK;
+	}
+
+	bool new_tzen;
+	COMMAND_PARSE_ENABLE(CMD_ARGV[1], new_tzen);
+
+	if (new_tzen == stm32l4_info->tzen) {
+		LOG_INFO("The requested TZEN is already programmed");
+		return ERROR_OK;
+	}
+
+	if (new_tzen) {
+		if (stm32l4_info->rdp != RDP_LEVEL_0) {
+			LOG_ERROR("TZEN can be set only when RDP level is 0");
+			return ERROR_FAIL;
+		}
+		retval = stm32l4_write_option(bank, stm32l4_info->flash_regs[STM32_FLASH_OPTR_INDEX],
+				FLASH_TZEN, FLASH_TZEN);
+	} else {
+		/* Deactivation of TZEN (from 1 to 0) is only possible when the RDP is
+		 * changing to level 0 (from level 1 to level 0 or from level 0.5 to level 0). */
+		if (stm32l4_info->rdp != RDP_LEVEL_1 && stm32l4_info->rdp != RDP_LEVEL_0_5) {
+			LOG_ERROR("Deactivation of TZEN is only possible when the RDP is changing to level 0");
+			return ERROR_FAIL;
+		}
+
+		retval = stm32l4_write_option(bank, stm32l4_info->flash_regs[STM32_FLASH_OPTR_INDEX],
+				RDP_LEVEL_0, FLASH_RDP_MASK | FLASH_TZEN);
+	}
+
 	if (retval != ERROR_OK)
 		return retval;
 
-	/* Set OBL_LAUNCH bit in CR -> system reset and option bytes reload,
-	 * but the RMs explicitly do *NOT* list this as power-on reset cause, and:
-	 * "Note: If the read protection is set while the debugger is still
-	 * connected through JTAG/SWD, apply a POR (power-on reset) instead of a system reset."
-	 */
-	retval = stm32l4_write_flash_reg_by_index(bank, STM32_FLASH_CR_INDEX, FLASH_OBL_LAUNCH);
+	return stm32l4_perform_obl_launch(bank);
+}
 
-	command_print(CMD, "stm32l4x option load completed. Power-on reset might be required");
+COMMAND_HANDLER(stm32l4_handle_option_load_command)
+{
+	if (CMD_ARGC != 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	/* Need to re-probe after change */
-	struct stm32l4_flash_bank *stm32l4_info = bank->driver_priv;
-	stm32l4_info->probed = false;
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (retval != ERROR_OK)
+		return retval;
 
-	return retval;
+	retval = stm32l4_perform_obl_launch(bank);
+	if (retval != ERROR_OK) {
+		command_print(CMD, "stm32l4x option load failed");
+		return retval;
+	}
+
+
+	command_print(CMD, "stm32l4x option load completed. Power-on reset might be required");
+
+	return ERROR_OK;
 }
 
 COMMAND_HANDLER(stm32l4_handle_lock_command)
@@ -2014,6 +2100,13 @@ static const struct command_registration stm32l4_exec_command_handlers[] = {
 		.help = "Write device option bit fields with provided value.",
 	},
 	{
+		.name = "trustzone",
+		.handler = stm32l4_handle_trustzone_command,
+		.mode = COMMAND_EXEC,
+		.usage = "<bank_id> [enable|disable]",
+		.help = "Configure TrustZone security",
+	},
+	{
 		.name = "wrp_info",
 		.handler = stm32l4_handle_wrp_info_command,
 		.mode = COMMAND_EXEC,