trusty: arm64-fiq: Re-enable FIQs after restoring cpu state

Make smc call, SMC_FC_FIQ_RESUME, into trusty to re-enable FIQs
after cpu state was lost in idle. The normal psci hooks do not
work for this as they run before Linux has restored its state.

Change-Id: I8a6e47724094ac338455c21a6bfff4e15e92938b
Signed-off-by: Arve Hjønnevåg <arve@android.com>

arch/arm64/kernel/sleep.S

@@ -138,6 +138,10 @@ ENTRY(_cpu_resume)
 	bl	kasan_unpoison_task_stack_below
 #endif
 
+#ifdef CONFIG_TRUSTY_FIQ
+	bl	trusty_fiq_cpu_resume
+#endif
+
 	ldp	x19, x20, [x29, #16]
 	ldp	x21, x22, [x29, #32]
 	ldp	x23, x24, [x29, #48]

drivers/trusty/trusty-fiq-arm64-glue.S

@@ -52,3 +52,8 @@ ENTRY(trusty_fiq_glue_arm64)
 	ldr	x0, =SMC_FC_FIQ_EXIT
 	smc	#0
 	b	.	/* should not get here */
+
+ENTRY(trusty_fiq_cpu_resume)
+	ldr	x0, =SMC_FC_FIQ_RESUME
+	smc	#0
+	ret

include/linux/trusty/smcall.h

@@ -124,6 +124,8 @@
 #define TRUSTY_API_VERSION_CURRENT	(3)
 #define SMC_FC_API_VERSION	SMC_FASTCALL_NR (SMC_ENTITY_SECURE_MONITOR, 11)
 
+#define SMC_FC_FIQ_RESUME	SMC_FASTCALL_NR (SMC_ENTITY_SECURE_MONITOR, 12)
+
 /* TRUSTED_OS entity calls */
 #define SMC_SC_VIRTIO_GET_DESCR	SMC_STDCALL_NR(SMC_ENTITY_TRUSTED_OS, 20)
 #define SMC_SC_VIRTIO_START	SMC_STDCALL_NR(SMC_ENTITY_TRUSTED_OS, 21)