sunxi-fel: handle H616/A133 secure-FEL handoff

On H616 and A133 with the secure boot fuse set, FEL starts in
non-secure state. The older direct SMC workaround is insufficient
because SMC enters monitor mode instead of returning the BROM FEL
command loop in secure SVC state. A133 also has its monitor vector
table cleared by NBROM.

Add a secure-SVC SMC thunk for this case and keep the existing global
startup workaround model. The thunk preserves the BROM SRAM workspace
using the same swap-table convention as the SPL thunk. Before issuing
SMC, it temporarily replaces the SMC vector instruction at MVBAR + 8
with "mov pc, lr". This returns to the instruction after SMC without
restoring SPSR_mon, so execution continues in monitor mode.

The thunk then restores the original vector instruction, clears
SCR.NS and MVBAR, sets the secure GIC state expected by the BROM,
copies the saved SVC SP/LR into the secure bank, switches to secure
SVC, and returns to the FEL command loop.

After the transition, the normal runtime probe sees secure state and
suppresses repeat application in that sunxi-fel process, so normal SID
reads and SPL execution use the existing code paths.

H616 and A133 select the secure-SVC SMC thunk path and gate the
workaround on a non-zero secure boot status word at SID base + 0xa0.
The zero-word runtime probe still has to match before the thunk is
applied, so non-secure boards and already-transitioned secure-FEL
sessions do not enter the secure path just because one check matches.

Signed-off-by: James Hilliard <james.hilliard1@gmail.com>

Author: jameshilliard

Makefile

@@ -164,7 +164,7 @@ SOC_INFO := soc_info.c soc_info.h
 FEL_LIB  := fel_lib.c fel_lib.h
 SPI_FLASH:= fel-spiflash.c fel-spiflash.h fel-remotefunc-spi-data-transfer.h
 
-sunxi-fel: fel.c fit_image.c thunks/fel-to-spl-thunk.h $(PROGRESS) $(SOC_INFO) $(FEL_LIB) $(SPI_FLASH)
+sunxi-fel: fel.c fit_image.c thunks/fel-to-spl-thunk.h thunks/fel-to-secure-svc-smc-thunk.h $(PROGRESS) $(SOC_INFO) $(FEL_LIB) $(SPI_FLASH)
 	$(CC) $(HOST_CFLAGS) $(LIBUSB_CFLAGS) $(ZLIB_CFLAGS) $(LIBFDT_CFLAGS) $(LDFLAGS) -o $@ \
 		$(filter %.c,$^) $(LIBS) $(LIBUSB_LIBS) $(ZLIB_LIBS) $(LIBFDT_LIBS)
 

fel.c

@@ -358,6 +358,10 @@ static uint32_t fel_to_spl_thunk[] = {
 	#include "thunks/fel-to-spl-thunk.h"
 };
 
+static uint32_t fel_to_secure_svc_smc_thunk[] = {
+	#include "thunks/fel-to-secure-svc-smc-thunk.h"
+};
+
 #define	DRAM_BASE		0x40000000
 #define	DRAM_SIZE		0x80000000
 
@@ -569,38 +573,106 @@ static bool aw_fel_needs_smc_workaround(feldev_handle *dev)
 	soc_info_t *soc_info = dev->soc_info;
 	uint32_t val;
 
-	/* Return if the SoC does not need this workaround */
+	if (soc_info->secure_boot_fuse_offset) {
+		if (!soc_info->sid_base)
+			return false;
+		aw_fel_read(dev, soc_info->sid_base +
+			    soc_info->secure_boot_fuse_offset,
+			    &val, sizeof(val));
+		if (!le32toh(val))
+			return false;
+	}
+
 	if (!soc_info->needs_smc_workaround_if_zero_word_at_addr)
 		return false;
-
-	/* This has less overhead than fel_readl_n() and may be good enough */
 	aw_fel_read(dev, soc_info->needs_smc_workaround_if_zero_word_at_addr,
-	            &val, sizeof(val));
+		    &val, sizeof(val));
+
+	return le32toh(val) == 0;
+}
+
+static void aw_fel_execute_secure_svc_smc_thunk(feldev_handle *dev)
+{
+	soc_info_t *soc_info = dev->soc_info;
+	const secure_svc_smc_info *smc_info = soc_info->secure_svc_smc;
+	const sram_swap_buffers *swap_buffers = soc_info->swap_buffers;
+	struct timespec req = { .tv_nsec = 250000000 }; /* 250ms */
+	uint32_t arm_code[] = {
+		htole32(0xe12fff1e), /* bx	lr		*/
+	};
+	uint32_t *thunk_buf, *p;
+	size_t thunk_size;
+	size_t i;
+
+	if (!smc_info)
+		pr_fatal("FEL thunk: missing secure-SVC SMC info\n");
+
+	for (i = 0; swap_buffers[i].size; i++)
+		;
+
+	thunk_size = sizeof(fel_to_secure_svc_smc_thunk) +
+		     4 * sizeof(uint32_t) +
+		     (i + 1) * sizeof(*swap_buffers);
+
+	if (thunk_size > soc_info->thunk_size)
+		pr_fatal("FEL thunk: bad size (need %zu, have %u)\n",
+			 thunk_size, soc_info->thunk_size);
 
-	return val == 0;
+	thunk_buf = malloc(thunk_size);
+	if (!thunk_buf)
+		pr_fatal("FEL thunk: failed to allocate buffer\n");
+	memcpy(thunk_buf, fel_to_secure_svc_smc_thunk,
+	       sizeof(fel_to_secure_svc_smc_thunk));
+
+	p = thunk_buf + ARRAY_SIZE(fel_to_secure_svc_smc_thunk);
+	*p++ = soc_info->spl_addr;
+	*p++ = smc_info->monitor_vector_addr;
+	*p++ = smc_info->gicc_base;
+	*p++ = smc_info->gicd_base;
+	memcpy(p, swap_buffers, (i + 1) * sizeof(*swap_buffers));
+
+	for (i = 0; i < thunk_size / sizeof(uint32_t); i++)
+		thunk_buf[i] = htole32(thunk_buf[i]);
+
+	aw_fel_write(dev, arm_code, soc_info->spl_addr, sizeof(arm_code));
+	aw_fel_write(dev, thunk_buf, soc_info->thunk_addr, thunk_size);
+	aw_fel_execute(dev, soc_info->thunk_addr);
+	free(thunk_buf);
+
+	/* TODO: Try to find and fix the bug, which needs this workaround */
+	nanosleep(&req, NULL);
 }
 
 /*
- * Issue a "smc #0" instruction. This brings a SoC booted in "secure boot"
- * state from the default non-secure FEL into secure FEL.
+ * Apply the "smc #0" workaround. This moves a secure-boot FEL session from
+ * the default non-secure state into secure state.
  * This crashes on devices using "non-secure boot", as the BROM does not
  * provide a handler address in MVBAR. So we have a runtime check.
+ * Some newer SoCs need to perform the SMC and return to FEL via a thunk,
+ * which handles the monitor-to-SVC transition details.
  */
 static void aw_apply_smc_workaround(feldev_handle *dev)
 {
 	soc_info_t *soc_info = dev->soc_info;
-	uint32_t arm_code[] = {
-		htole32(0xe1600070), /* smc	#0	*/
-		htole32(0xe12fff1e), /* bx	lr	*/
-	};
 
-	/* Return if the workaround is not needed or has been already applied */
+	/* Return if the workaround is not needed */
 	if (!aw_fel_needs_smc_workaround(dev))
 		return;
 
-	pr_info("Applying SMC workaround... ");
-	aw_fel_write(dev, arm_code, soc_info->scratch_addr, sizeof(arm_code));
-	aw_fel_execute(dev, soc_info->scratch_addr);
+	if (soc_info->smc_workaround == SMC_WORKAROUND_SECURE_SVC_SMC_THUNK) {
+		pr_info("Applying SMC workaround via secure-SVC SMC thunk... ");
+		aw_fel_execute_secure_svc_smc_thunk(dev);
+	} else {
+		uint32_t arm_code[] = {
+			htole32(0xe1600070), /* smc	#0		*/
+			htole32(0xe12fff1e), /* bx	lr		*/
+		};
+
+		pr_info("Applying SMC workaround... ");
+		aw_fel_write(dev, arm_code, soc_info->scratch_addr,
+			     sizeof(arm_code));
+		aw_fel_execute(dev, soc_info->scratch_addr);
+	}
 	pr_info(" done.\n");
 }
 
@@ -1388,7 +1460,7 @@ int main(int argc, char **argv)
 	 */
 	handle = feldev_open(busnum, devnum, AW_USB_VENDOR_ID, AW_USB_PRODUCT_ID);
 
-	/* Some SoCs need the SMC workaround to enter the secure boot mode */
+	/* Some SoCs need the SMC workaround to enter secure state */
 	aw_apply_smc_workaround(handle);
 
 	/* Handle command-style arguments, in order of appearance */

soc_info.c

@@ -156,6 +156,12 @@ sram_swap_buffers a133_sram_swap_buffers[] = {
 	{ .size = 0 }  /* End of the table */
 };
 
+static const secure_svc_smc_info h616_a133_secure_svc_smc = {
+	.monitor_vector_addr = 0x000300c0,
+	.gicc_base           = 0x03022000,
+	.gicd_base           = 0x03021000,
+};
+
 /*
  * R329 has no SRAM A1, but a huge SRAM A2 at 0x100000. SPL and BROM uses
  * this SRAM A2's first part like how other SoCs use SRAM A1. The sp and
@@ -565,6 +571,10 @@ soc_info_t soc_info_table[] = {
 		.rvbar_reg    = 0x09010040,
 		.rvbar_reg_alt= 0x08100040,
 		.ver_reg      = 0x03000024,
+		.needs_smc_workaround_if_zero_word_at_addr = 0x03006240,
+		.secure_boot_fuse_offset = 0xa0,
+		.smc_workaround = SMC_WORKAROUND_SECURE_SVC_SMC_THUNK,
+		.secure_svc_smc = &h616_a133_secure_svc_smc,
 		.watchdog     = &wd_h6_compat,
 	},{
 		.soc_id       = 0x1851, /* Allwinner R329 */
@@ -644,8 +654,10 @@ soc_info_t soc_info_table[] = {
 		.sid_offset   = 0x200,
 		.sid_sections = generic_2k_sid_maps,
 		.rvbar_reg    = 0x08100040,
-		.needs_smc_workaround_if_zero_word_at_addr = 0x100004,
-		.smc_workaround = SMC_WORKAROUND_DIRECT_SMC,
+		.needs_smc_workaround_if_zero_word_at_addr = 0x03006240,
+		.secure_boot_fuse_offset = 0xa0,
+		.smc_workaround = SMC_WORKAROUND_SECURE_SVC_SMC_THUNK,
+		.secure_svc_smc = &h616_a133_secure_svc_smc,
 		.watchdog     = &wd_h6_compat,
 	},{
 		.swap_buffers = NULL /* End of the table */

soc_info.h

@@ -108,17 +108,30 @@ typedef struct {
  *   be accessible from non-secure world.
  * - No RMR trigger on ARMv8 cores to bring the core into AArch64.
  * On older SoCs, a simple "smc" call returns with the NS bit cleared,
- * so access to all secure peripherals is suddenly possible.
+ * so access to all secure peripherals is suddenly possible. Newer SoCs
+ * may need a secure-SVC thunk to handle the monitor-to-SVC transition
+ * after the SMC call before returning to FEL.
  * The 'needs_smc_workaround_if_zero_word_at_addr' field can be used to
  * have a check for this condition (reading from restricted addresses
  * typically returns zero) and then activate the SMC workaround if needed.
+ * The 'secure_boot_fuse_offset' field can be used when the SoC has an
+ * explicit readable secure boot status word at sid_base + offset. It is a
+ * gate only; a separate runtime state probe is still required to avoid
+ * applying the workaround on every invocation.
  * The 'smc_workaround' field selects how to apply the workaround once the
  * runtime checks say that it is needed.
  */
 typedef enum {
 	SMC_WORKAROUND_DIRECT_SMC,
+	SMC_WORKAROUND_SECURE_SVC_SMC_THUNK,
 } smc_workaround_t;
 
+typedef struct {
+	uint32_t monitor_vector_addr;
+	uint32_t gicc_base;
+	uint32_t gicd_base;
+} secure_svc_smc_info;
+
 typedef struct {
 	uint32_t           soc_id;       /* ID of the SoC */
 	const char         *name;        /* human-readable SoC name string */
@@ -140,8 +153,11 @@ typedef struct {
 	bool               icache_fix;
 	/* Use SMC workaround (enter secure mode) if can't read from this address */
 	uint32_t           needs_smc_workaround_if_zero_word_at_addr;
+	/* Require non-zero sid_base + offset before applying SMC workaround */
+	uint32_t           secure_boot_fuse_offset;
 	/* How to apply the SMC workaround */
 	smc_workaround_t   smc_workaround;
+	const secure_svc_smc_info *secure_svc_smc;
 	uint32_t           sram_size;	/* Usable contiguous SRAM at spl_addr */
 	sram_swap_buffers *swap_buffers;
 } soc_info_t;

thunks/Makefile

@@ -3,6 +3,7 @@
 #
 
 SPL_THUNK := fel-to-spl-thunk.h
+SPL_THUNK += fel-to-secure-svc-smc-thunk.h
 THUNKS := clrsetbits.h
 THUNKS += memcpy.h
 THUNKS += readl_writel.h

thunks/README.md

@@ -8,10 +8,10 @@ usually via `sunxi-fel`.
 
 Normally you don't need to change or (re)build anything within this folder.
 Currently our main build process (via the parent directory's _Makefile_)
-only includes `fel-to-spl-thunk.h` directly. Other _.h_ files are provided
-**just for reference**. The main purpose of this folder is simply keeping
-track of _.S_ sources, to help with possible future maintenance of the
-various code snippets.
+includes `fel-to-spl-thunk.h` and `fel-to-secure-svc-smc-thunk.h` directly.
+Other _.h_ files are provided **just for reference**. The main purpose of this
+folder is simply keeping track of _.S_ sources, to help with possible future
+maintenance of the various code snippets.
 
 Please note that any files lacking explicit license information are intended
 to be covered by the project's [overall license](../LICENSE.md) (GPLv2).

thunks/fel-to-secure-svc-smc-thunk.S

@@ -0,0 +1,129 @@
+/* secure-FEL thunk returning from SMC in monitor mode. */
+
+.arm
+
+BUF1		.req	r0
+BUF2		.req	r1
+TMP1		.req	r2
+TMP2		.req	r3
+SWAPTBL		.req	r4
+ORIG_VECTOR	.req	r5
+BUFSIZE		.req	r6
+ENTRY_ADDR	.req	r8
+MON_VECTOR	.req	r9
+SAVED_SP	.req	r10
+SAVED_LR	.req	r11
+
+entry_point:
+	b	setup_stack
+
+stack_begin:
+	.space	32, 0xff
+stack_end:
+	nop
+
+	/* Walk the table and swap all buffers. */
+swap_all_buffers:
+	adr	SWAPTBL,   appended_data + 16
+swap_next_buffer:
+	ldr	BUF1,      [SWAPTBL],  #4
+	ldr	BUF2,      [SWAPTBL],  #4
+	ldr	BUFSIZE,   [SWAPTBL],  #4
+	cmp	BUFSIZE,   #0
+	bxeq    lr
+swap_next_word:
+	ldr	TMP1,      [BUF1]
+	ldr	TMP2,      [BUF2]
+	subs	BUFSIZE,   BUFSIZE,    #4
+	str	TMP1,      [BUF2],     #4
+	str	TMP2,      [BUF1],     #4
+	bne	swap_next_word
+	b	swap_next_buffer
+
+setup_stack:
+	ldr	ENTRY_ADDR, appended_data
+	ldr	MON_VECTOR, appended_data + 4
+	adr	BUF1,      stack_end
+	str	sp,        [BUF1, #-4]!
+	mov	sp,        BUF1
+	mrs	TMP1,      cpsr
+	push	{TMP1, lr}
+
+	/* Disable IRQ and FIQ. */
+	orr	TMP1,      #0xc0
+	msr	cpsr_c,    TMP1
+
+	/* Check if the instructions or data cache is enabled. */
+	mrc	p15, 0, TMP1, c1, c0, 0
+	tst	TMP1,      #(1 << 2)
+	tsteq	TMP1,      #(1 << 12)
+	bne	return_to_fel_noswap
+
+	bl	swap_all_buffers
+	bl	call_entry
+	b	return_to_secure_fel
+
+return_to_secure_fel:
+	bl	swap_all_buffers
+
+return_to_fel_noswap:
+	pop	{TMP1, lr}
+	msr	cpsr_c,    TMP1
+	ldr     sp,        [sp]
+	bx	lr
+
+call_entry:
+	/* Make the monitor SMC vector return without restoring SPSR_mon. */
+	ldr	ORIG_VECTOR, [MON_VECTOR, #8]
+	ldr	TMP1,      monitor_return
+	str	TMP1,      [MON_VECTOR, #8]
+
+	/* Save the non-secure SVC SP/LR for the secure bank below. */
+	mcr	p15, 0, TMP1, c7, c10, 4	/* CP15DSB */
+	mrc	p15, 0, TMP1, c0, c0, 0		/* read MIDR */
+	and	TMP1, TMP1, #(0xf << 16)	/* architecture */
+	cmp	TMP1, #(0x6 << 16)		/* ARMv5TEJ */
+	mcrgt	p15, 0, TMP1, c7, c5, 4		/* CP15ISB, if > ARMv5TEJ */
+	mov	SAVED_SP,  sp
+	mov	SAVED_LR,  lr
+	.word	0xe1600070			/* smc	#0 */
+
+	/* The monitor SMC vector returns here without changing mode. */
+	str	ORIG_VECTOR, [MON_VECTOR, #8]
+	mov	TMP1,      #0
+	mcr	p15, 0, TMP1, c1, c1, 0		/* SCR = secure */
+	mcr	p15, 0, TMP1, c12, c0, 1	/* MVBAR = 0 */
+
+	ldr	TMP1,      appended_data + 8
+	mov	TMP2,      #0xf
+	str	TMP2,      [TMP1]
+	mov	TMP2,      #0xf8
+	str	TMP2,      [TMP1, #4]
+	ldr	TMP1,      appended_data + 12
+	mov	TMP2,      #3
+	str	TMP2,      [TMP1]
+
+	.word	0xe123f30a			/* msr	SP_svc, SAVED_SP */
+	.word	0xe122f30b			/* msr	LR_svc, SAVED_LR */
+	.word	0xf57ff06f			/* isb */
+	mrs	TMP1,      cpsr
+	bic	TMP1,      TMP1, #0x1f
+	orr	TMP1,      TMP1, #0x13
+	msr	cpsr_fc,   TMP1			/* switch to secure-SVC mode */
+	bx	ENTRY_ADDR
+
+monitor_return:
+	.word	0xe1a0f00e			/* mov	pc, lr */
+
+appended_data:
+/*
+ * The appended data uses the following format:
+ *
+ *     struct {
+ *         uint32_t          entry_addr;
+ *         uint32_t          monitor_vector_addr;
+ *         uint32_t          gicc_ctlr_addr;
+ *         uint32_t          gicd_ctlr_addr;
+ *         sram_swap_buffers swaptbl[];
+ *     };
+ */