/*
 * Copyright (c) 2013-2015, Google Inc. All rights reserved
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files
 * (the "Software"), to deal in the Software without restriction,
 * including without limitation the rights to use, copy, modify, merge,
 * publish, distribute, sublicense, and/or sell copies of the Software,
 * and to permit persons to whom the Software is furnished to do so,
 * subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
 * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
 * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
 * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
 * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#include <asm.h>
#include <lib/sm/monitor.h>
#include <lib/sm/smcall.h>
#include <lib/sm/sm_err.h>

#include <kernel/vm.h>

/*
 * Monitor stacks:
 *  secure context
 *  non-secure context
 *
 * Context:
 *  mon_lr
 *  usr/sys_sp, usr/sys_lr
 *  und_spsr, und_sp, und_lr
 *  abt_spsr, abt_sp, abt_lr
 *  svc_spsr, svc_sp, svc_lr
 *  irq_spsr, irq_sp, irq_lr
 *  fiq_spsr, fiq_r8-fiq_lr
 *  r4-r11, lr
 *  r0-r3, ip
 *  lr, spsr
 *
 */

#define CONTEXT_SIZE_RETURN (2 * 4)
#define CONTEXT_SIZE_SCRATCH_GEN_REGS ((5 + 9) * 4)
#define CONTEXT_SIZE_MODE_REGS_MON_RETURN ((8 + 3 + 3 + 3 + 3 + 2 + 1) * 4)
#define CONTEXT_SIZE (CONTEXT_SIZE_RETURN + CONTEXT_SIZE_SCRATCH_GEN_REGS + CONTEXT_SIZE_MODE_REGS_MON_RETURN)
#define MONITOR_STACK_SIZE (CONTEXT_SIZE * 2)

.macro SAVE_CONTEXT_RETURN
 srsdb sp!, #MODE_MON /* srsfd alias not recognized by current assembler */
.endm

.macro SAVE_CONTEXT_GEN_REGS
 push {r4-r11, lr}
.endm

.macro SAVE_CONTEXT_SCRATCH_REGS
 push {r0-r3, ip}
.endm

.macro SAVE_CONTEXT
 SAVE_CONTEXT_RETURN
 SAVE_CONTEXT_SCRATCH_REGS
 SAVE_CONTEXT_GEN_REGS
.endm

.macro SAVE_CONTEXT_MODE_REGS
 mov r4, sp

 cps #MODE_FIQ
 mrs r5, spsr
 stmfd r4!, {r5, r8-lr}

 cps #MODE_IRQ
 mrs r5, spsr
 stmfd r4!, {r5, sp, lr}

 cps #MODE_SVC
 mrs r5, spsr
 stmfd r4!, {r5, sp, lr}

 cps #MODE_ABT
 mrs r5, spsr
 stmfd r4!, {r5, sp, lr}

 cps #MODE_UND
 mrs r5, spsr
 stmfd r4!, {r5, sp, lr}

 cps #MODE_SYS
 stmfd r4!, {sp, lr}

 cps #MODE_MON
 mov sp, r4
.endm

.macro SAVE_RETURN_ADDR reg
 push {\reg}
.endm

.macro RESTORE_RETURN_ADDR reg
 pop {\reg}
.endm

.macro RESTORE_CONTEXT_MODE_REGS
 mov r4, sp

 cps #MODE_SYS
 ldmfd r4!, {sp, lr}

 cps #MODE_UND
 ldmfd r4!, {r5, sp, lr}
 msr spsr, r5

 cps #MODE_ABT
 ldmfd r4!, {r5, sp, lr}
 msr spsr, r5

 cps #MODE_SVC
 ldmfd r4!, {r5, sp, lr}
 msr spsr, r5

 cps #MODE_IRQ
 ldmfd r4!, {r5, sp, lr}
 msr spsr, r5

 cps #MODE_FIQ
 ldmfd r4!, {r5, r8-lr}
 msr spsr, r5

 cps #MODE_MON
 mov sp, r4
.endm

.macro RESTORE_CONTEXT_GEN_REGS
 pop {r4-r11, lr}
.endm

.macro RESTORE_CONTEXT_SCRATCH_REGS
 pop {r0-r3, ip}
.endm

.macro RESTORE_CONTEXT_AND_RETURN
 RESTORE_CONTEXT_GEN_REGS
 add sp, sp, #5 * 4
 rfefd sp!
.endm

.p2align 5
.globl monitor_vector_table
monitor_vector_table:
 nop    /* RESET */
 b .   /* UNDEF */
 b .Lmon_smcall  /* SWI */
 b .   /* IABORT */
 b .   /* DABORT */
 nop    /* reserved */
 b .   /* IRQ */
 b .Lmon_fiq_entry  /* FIQ */

/*
 * Called in monitor mode
 * return address in mon-lr
 * ns-return address in svc-lr
 */

FUNCTION(monitor_reset)
 /* figure out our cpu number */
 mrc     p15, 0, r11, c0, c0, 5 /* read MPIDR */

 /* mask off the bottom 12 bits to test cluster number:cpu number */
 ubfx    r11, r11, #0, #12

 /* return if cpu >= SMP_MAX_CPUS */
 cmp r11, #SMP_MAX_CPUS
 bxge lr

 /* find cpu specifc stack offset */
 mov ip, #MONITOR_STACK_SIZE
 mul r11, r11,ip

 adr sp, monitor_reset /* sp = paddr */
 ldr ip, =monitor_reset /* ip = vaddr */
 sub ip, sp, ip  /* ip = phys_offset */
 ldr sp, =mon_ns_stack_top /* sp = ns_stack vaddr */
 add sp, sp, ip  /* sp = ns_stack paddr */

 /* apply per-cpu stack offset */
 sub sp, sp, r11

 /* Save non-secure return address, mode and registers */
 cps #MODE_SVC
 msr spsr_cfsx, #MODE_SVC_IRQ_DISABLED
 SAVE_CONTEXT_RETURN
 cps #MODE_MON

 SAVE_CONTEXT_SCRATCH_REGS
 SAVE_CONTEXT_GEN_REGS
 SAVE_CONTEXT_MODE_REGS
 ldr r4, =platform_mon_initial_ns_return
 SAVE_RETURN_ADDR r4

 sub sp, sp, ip /* sp = sp vaddr */

 /* Initialize NS mode CPU context registers */
 mrc p15, 0, r4, c12, c0, 0 /* r4 = VBAR */
 mrc p15, 0, r5, c2, c0, 0 /* r5 = TTBR0 */
 mrc p15, 0, r6, c2, c0, 1 /* r6 = TTBR1 */
 mrc p15, 0, r7, c3, c0, 0 /* r7 = DACR */
 mrc p15, 0, r8, c1, c0, 0 /* r8 = SCTLR */

 SWITCH_SCR_TO_NONSECURE r9

 mcr p15, 0, r4, c12, c0, 0 /* VBAR = r4 */
 mcr p15, 0, r5, c2, c0, 0 /* TTBR0 = r5 */
 mcr p15, 0, r6, c2, c0, 1 /* TTBR1 = r6 */
 mcr p15, 0, r7, c3, c0, 0 /* DACR = r7 */
 mcr p15, 0, r8, c1, c0, 0 /* SCTLR = r8 */

 SWITCH_SCR_TO_SECURE r9

 bx lr

FUNCTION(monitor_init_mmu_on)
 bx lr

.Lmon_invalid_ns_return_addr:
 b .

FUNCTION(mon_initial_ns_return)
 cmp lr, #~0
 strne lr, [sp, #CONTEXT_SIZE_SCRATCH_GEN_REGS]
 b .Lmon_smcall_initial_ns_return

/* direction in flags */
.Lmon_smcall_from_secure:
 adreq lr, .Lmon_smcall_secure_return
.Lmon_context_switch:
 SAVE_CONTEXT_MODE_REGS
 SAVE_RETURN_ADDR lr

 addeq sp, sp, #(CONTEXT_SIZE) /* secure => non-secure */
 subne sp, sp, #(CONTEXT_SIZE) /* non-secure => secure */

 RESTORE_RETURN_ADDR lr
 RESTORE_CONTEXT_MODE_REGS
 bx lr

.Lmon_smcall:
 SAVE_CONTEXT

 mrc p15, 0, r4, c1, c1, 0 /* r4 = SCR */
 tst r4, #0x1
 beq .Lmon_smcall_from_secure

 SWITCH_SCR_TO_SECURE r4
 bl .Lmon_context_switch
.Lmon_smcall_initial_ns_return:
.weak platform_mon_initial_ns_return
platform_mon_initial_ns_return:
 RESTORE_CONTEXT_GEN_REGS

 ldr lr, [sp]

 /* Handle SMC_FC_FIQ_EXIT from non-secure */
 cmp lr, #SMC_FC_FIQ_EXIT
 beq .Lmon_fiq_exit_return

 SWITCH_SCR_TO_NONSECURE r3
 mov r0, r1 /* secure os passes the return code in r1 */

 add sp, sp, #5 * 4
 rfefd sp!


.Lmon_smcall_secure_return:
 RESTORE_CONTEXT_AND_RETURN

.Lmon_fiq_entry:

 SAVE_CONTEXT

 mrc p15, 0, r4, c1, c1, 0 /* r4 = SCR */
 tst r4, #0x1
 beq . /* fiqs are not trapped in secure mode */

 ldr r0, =SMC_FC_FIQ_ENTER

 SWITCH_SCR_TO_SECURE r4
 bl .Lmon_context_switch
 SWITCH_SCR_TO_NONSECURE r4

 RESTORE_CONTEXT_GEN_REGS
 ldr lr, [sp, #(5 * 4)]

 cmp r1, #0 /* secure os passes the return code in r1 */
 subne lr, lr, #4
 bne .Lmon_fiq_return

 ldr r0, [sp, #(6 * 4)] /* load saved spsr */
 mov r1, lr

 mov r2, #0x91 /* fiq mode with IRQ disabled */
 str r2, [sp, #(6 * 4)] /* store spsr */

 mrc p15, 0, r3, c1, c0, 0 /* r3 = Non-secure SCTLR */
 tst r3, #(1 << 13) /* Non-secure SCTLR.V */
 ldrne lr, =0xffff0000
 mrceq p15, 0, lr, c12, c0, 0 /* lr = VBAR (if not using Hivecs) */
 add lr, lr, #0x1c /* fiq vector offset */

 SWITCH_SCR_TO_SECURE r2

 /* Set fiq mode lr and spsr */
 cps #MODE_FIQ
 msr spsr_cfsx, r0
 mov lr, r1
 cps #MODE_MON

 SWITCH_SCR_TO_NONSECURE r2

.Lmon_fiq_return:
 str lr, [sp, #(5 * 4)]
 RESTORE_CONTEXT_SCRATCH_REGS
 rfefd sp!

.Lmon_fiq_exit_return:
 /* Retrieve FIQ mode spsr, lr and restore r0 from FIQ mode r12 */
 cps #MODE_FIQ

 mov r0, r12 /* restore r0 used for smc number */
 mrs r1, spsr
 mov r2, lr

 cps #MODE_MON

 str r2, [sp, #(5 * 4)] /* save return addr */

 /* don't allow return to monitor mode */
 and r2, r1, #0x1f
 cmp r2, #MODE_MON
 moveq r1, #MODE_UND

 str r1, [sp, #(6 * 4)] /* store spsr */

 SWITCH_SCR_TO_NONSECURE r3

 mov lr, r0
 RESTORE_CONTEXT_SCRATCH_REGS
 mov r0, lr
 rfefd sp!

.data
.align 3

/* Monitor stack for primary cpu */
LOCAL_DATA(mon_ns_stack)
 .skip MONITOR_STACK_SIZE * SMP_MAX_CPUS
LOCAL_DATA(mon_ns_stack_top)