ARM setting up secure mode vector table

[Tinchu]

Hi,

Apologies in advance for extra CC list.

I'm trying to hack linux kernel (v4.14 LTS) for Raspberry PI 3 (ARM32 build), to
enable early boot code switch to secure mode using SMC #0 instruction and return
(and later tinker with some secure mode registers).

While this may generally not be possible, it seems it just might for this platform
(i can do this on a baremetal library, but not kernel proper. Apparently the
current BCM firmware sets up secure, non secure  and monitor vector base addresses
to 0x00000000 before calling into zImage and all mem is writable for non secure
mode. See the gory details in ultibo pascal library code comments [1]. The trick
is to copy a vector table with SMC vector to address 0. I'm suspecting this not
working for linux as having to write the code in true position independent way.

Here's a rough hack to arch/arm/kernel/hyp-stub.S
I'm simply hacking in a vector entry into existing hyp vector table and then
copying it over to 0 (I've tried using a completely different table, which fails
in same way).

+	.macro sec_vec_install_run
+
+		mrc p15, #0, r4, c12, c0, #0
+		ldr r5, .LSecureVectorTable
+
+		/* 8 bytes of vector */
+		ldmia r5!, {r6-r7}
+		stmia r4!, {r6-r7}
+
+		ldmia r5!, {r6-r7}
+		stmia r4!, {r6-r7}
+
+		ldmia r5!, {r6-r7}
+		stmia r4!, {r6-r7}
+
+		ldmia r5!, {r6-r7}
+		stmia r4!, {r6-r7}
+
+		/* 1 word for secondary table */
+		ldmia r5!, {r6-r7}
+		stmia r4!, {r6-r7}
+
+		/*Clean Data Cache MVA */
+		mov r5, #0
+		mcr p15, #0, r5, cr7, cr10, #1
+
+		dsb
+
+		//Invalidate Instruction Cache
+		mov r5, #0
+		mcr p15, #0, r5, cr7, cr5, #0
+
+		//Flush Branch Target Cache
+		mov r5, #0
+		mcr p15, #0, r5, cr7, cr5, #6
+
+		dsb
+		isb
+
+		.arch_extension sec
+		smc #0
+
+	.endm


ENTRY(__hyp_stub_install_secondary)

+	sec_vec_install_run
...

+smc_hdlr:
+	/* do stuff later */
+	ret	lr
+ENDPROC(smc_hdlr)

.align 5
ENTRY(__hyp_stub_vectors)
__hyp_stub_reset:	W(b)	.
__hyp_stub_und:		W(b)	.
+ __hyp_stub_svc:	ldr pc, .Lhdlr2
- __hyp_stub_svc:	W(b)	.

...

+.Lhdlr2:
+	.word smc_hdlr
+.LSecureVectorTable:
+	.long  __hyp_stub_vectors


Can someone please tell me what part of code is wrong. Pretty much similar code
works in baremetal setup.

TIA

[1] https://github.com/ultibohub/Core/blob/master/source/rtl/ultibo/core/bootrpi2.pas