stm32mp: stm32prog: add otp update support

Add a virtual partition to update the STM32MP15x OTP based
on SMC service provided by TF-A.

Signed-off-by: Patrick Delaunay <patrick.delaunay@st.com>
Reviewed-by: Patrice Chotard <patrice.chotard@st.com>

3 files changed, 151 insertions, 1 deletions

diff --git a/arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog.c b/arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog.c
index 3573c04..cd826db 100644
--- a/arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog.c
+++ b/arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog.c
@@ -9,6 +9,7 @@
 #include <malloc.h>
 #include <mmc.h>
 #include <part.h>
+#include <asm/arch/stm32mp1_smc.h>
 #include <dm/uclass.h>
 #include <jffs2/load_kernel.h>
 #include <linux/list.h>
@@ -1106,7 +1107,7 @@ static int dfu_init_entities(struct stm32prog_data *data)
 	struct dfu_entity *dfu;
 	int alt_nb;
 
-	alt_nb = 1; /* number of virtual = CMD */
+	alt_nb = 2; /* number of virtual = CMD, OTP*/
 	if (data->part_nb == 0)
 		alt_nb++;  /* +1 for FlashLayout */
 	else
@@ -1154,6 +1155,9 @@ static int dfu_init_entities(struct stm32prog_data *data)
 	if (!ret)
 		ret = stm32prog_alt_add_virt(dfu, "virtual", PHASE_CMD, 512);
 
+	if (!ret)
+		ret = stm32prog_alt_add_virt(dfu, "OTP", PHASE_OTP, 512);
+
 	if (ret)
 		stm32prog_err("dfu init failed: %d", ret);
 	puts("done\n");
@@ -1164,6 +1168,123 @@ static int dfu_init_entities(struct stm32prog_data *data)
 	return ret;
 }
 
+int stm32prog_otp_write(struct stm32prog_data *data, u32 offset, u8 *buffer,
+			long *size)
+{
+	pr_debug("%s: %x %lx\n", __func__, offset, *size);
+
+	if (!data->otp_part) {
+		data->otp_part = memalign(CONFIG_SYS_CACHELINE_SIZE, OTP_SIZE);
+		if (!data->otp_part)
+			return -ENOMEM;
+	}
+
+	if (!offset)
+		memset(data->otp_part, 0, OTP_SIZE);
+
+	if (offset + *size > OTP_SIZE)
+		*size = OTP_SIZE - offset;
+
+	memcpy((void *)((u32)data->otp_part + offset), buffer, *size);
+
+	return 0;
+}
+
+int stm32prog_otp_read(struct stm32prog_data *data, u32 offset, u8 *buffer,
+		       long *size)
+{
+#ifndef CONFIG_ARM_SMCCC
+	stm32prog_err("OTP update not supported");
+
+	return -1;
+#else
+	int result = 0;
+
+	pr_debug("%s: %x %lx\n", __func__, offset, *size);
+	/* alway read for first packet */
+	if (!offset) {
+		if (!data->otp_part)
+			data->otp_part =
+				memalign(CONFIG_SYS_CACHELINE_SIZE, OTP_SIZE);
+
+		if (!data->otp_part) {
+			result = -ENOMEM;
+			goto end_otp_read;
+		}
+
+		/* init struct with 0 */
+		memset(data->otp_part, 0, OTP_SIZE);
+
+		/* call the service */
+		result = stm32_smc_exec(STM32_SMC_BSEC, STM32_SMC_READ_ALL,
+					(u32)data->otp_part, 0);
+		if (result)
+			goto end_otp_read;
+	}
+
+	if (!data->otp_part) {
+		result = -ENOMEM;
+		goto end_otp_read;
+	}
+
+	if (offset + *size > OTP_SIZE)
+		*size = OTP_SIZE - offset;
+	memcpy(buffer, (void *)((u32)data->otp_part + offset), *size);
+
+end_otp_read:
+	pr_debug("%s: result %i\n", __func__, result);
+
+	return result;
+#endif
+}
+
+int stm32prog_otp_start(struct stm32prog_data *data)
+{
+#ifndef CONFIG_ARM_SMCCC
+	stm32prog_err("OTP update not supported");
+
+	return -1;
+#else
+	int result = 0;
+	struct arm_smccc_res res;
+
+	if (!data->otp_part) {
+		stm32prog_err("start OTP without data");
+		return -1;
+	}
+
+	arm_smccc_smc(STM32_SMC_BSEC, STM32_SMC_WRITE_ALL,
+		      (u32)data->otp_part, 0, 0, 0, 0, 0, &res);
+
+	if (!res.a0) {
+		switch (res.a1) {
+		case 0:
+			result = 0;
+			break;
+		case 1:
+			stm32prog_err("Provisioning");
+			result = 0;
+			break;
+		default:
+			pr_err("%s: OTP incorrect value (err = %ld)\n",
+			       __func__, res.a1);
+			result = -EINVAL;
+			break;
+		}
+	} else {
+		pr_err("%s: Failed to exec svc=%x op=%x in secure mode (err = %ld)\n",
+		       __func__, STM32_SMC_BSEC, STM32_SMC_WRITE_ALL, res.a0);
+		result = -EINVAL;
+	}
+
+	free(data->otp_part);
+	data->otp_part = NULL;
+	pr_debug("%s: result %i\n", __func__, result);
+
+	return result;
+#endif
+}
+
 /* copy FSBL on NAND to improve reliability on NAND */
 static int stm32prog_copy_fsbl(struct stm32prog_part_t *part)
 {
@@ -1451,6 +1572,7 @@ void stm32prog_clean(struct stm32prog_data *data)
 	/* clean */
 	dfu_free_entities();
 	free(data->part_array);
+	free(data->otp_part);
 	free(data->header_data);
 }
 
@@ -1460,6 +1582,12 @@ void dfu_flush_callback(struct dfu_entity *dfu)
 	if (!stm32prog_data)
 		return;
 
+	if (dfu->dev_type == DFU_DEV_VIRT) {
+		if (dfu->data.virt.dev_num == PHASE_OTP)
+			stm32prog_otp_start(stm32prog_data);
+		return;
+	}
+
 	if (dfu->dev_type == DFU_DEV_RAM) {
 		if (dfu->alt == 0 &&
 		    stm32prog_data->phase == PHASE_FLASHLAYOUT) {
diff --git a/arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog.h b/arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog.h
index 1880b16..6024657 100644
--- a/arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog.h
+++ b/arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog.h
@@ -11,12 +11,15 @@
 #define PHASE_FIRST_USER	0x10
 #define PHASE_LAST_USER		0xF0
 #define PHASE_CMD		0xF1
+#define PHASE_OTP		0xF2
 #define PHASE_END		0xFE
 #define PHASE_RESET		0xFF
 #define PHASE_DO_RESET		0x1FF
 
 #define DEFAULT_ADDRESS		0xFFFFFFFF
 
+#define OTP_SIZE		1024
+
 enum stm32prog_target {
 	STM32PROG_NONE,
 	STM32PROG_MMC,
@@ -116,6 +119,7 @@ struct stm32prog_data {
 	u32			offset;
 	char			error[255];
 	struct stm32prog_part_t	*cur_part;
+	u32			*otp_part;
 
 	/* STM32 header information */
 	struct raw_header_s	*header_data;
@@ -124,6 +128,13 @@ struct stm32prog_data {
 
 extern struct stm32prog_data *stm32prog_data;
 
+/* OTP access */
+int stm32prog_otp_write(struct stm32prog_data *data, u32 offset,
+			u8 *buffer, long *size);
+int stm32prog_otp_read(struct stm32prog_data *data, u32 offset,
+		       u8 *buffer, long *size);
+int stm32prog_otp_start(struct stm32prog_data *data);
+
 /* generic part*/
 u8 stm32prog_header_check(struct raw_header_s *raw_header,
 			  struct image_header_s *header);
diff --git a/arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog_usb.c b/arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog_usb.c
index ed2cdbc..4a4b4d3 100644
--- a/arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog_usb.c
+++ b/arch/arm/mach-stm32mp/cmd_stm32prog/stm32prog_usb.c
@@ -130,6 +130,10 @@ int stm32prog_write_medium_virt(struct dfu_entity *dfu, u64 offset,
 	switch (dfu->data.virt.dev_num) {
 	case PHASE_CMD:
 		return stm32prog_cmd_write(offset, buf, len);
+
+	case PHASE_OTP:
+		return stm32prog_otp_write(stm32prog_data, (u32)offset,
+					   buf, len);
 	}
 	*len = 0;
 	return 0;
@@ -144,6 +148,10 @@ int stm32prog_read_medium_virt(struct dfu_entity *dfu, u64 offset,
 	switch (dfu->data.virt.dev_num) {
 	case PHASE_CMD:
 		return stm32prog_cmd_read(offset, buf, len);
+
+	case PHASE_OTP:
+		return stm32prog_otp_read(stm32prog_data, (u32)offset,
+					  buf, len);
 	}
 	*len = 0;
 	return 0;
@@ -162,6 +170,9 @@ int stm32prog_get_medium_size_virt(struct dfu_entity *dfu, u64 *size)
 	case PHASE_CMD:
 		*size = 512;
 		break;
+	case PHASE_OTP:
+		*size = OTP_SIZE;
+		break;
 	}
 
 	return 0;