/*
 * Copyright (c) 2013, 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>

#if !LIB_SM_CUSTOM_SCHED_NONSECURE
/* sm_sched_nonsecure(uint32_t retval, struct smc32_args *args) */
FUNCTION(sm_sched_nonsecure)
 push {r4-r7}
 push {r1, lr}
 mov r1, r0

#if LIB_SM_WITH_PLATFORM_NS_RETURN
 bl platform_ns_return
#else
 ldr r0, =SMC_SC_NS_RETURN
 smc #0
#endif

 pop {r12, lr}

 /* Write struct smc32_args (top bits of client_id are left unchanged */
 stmia r12, {r0-r3, r7}

 pop {r4-r7}

 bx lr
#endif

#if LIB_SM_WITH_PLATFORM_RESET
FUNCTION(sm_reset)
#else
FUNCTION(platform_reset)
#endif
#if WITH_LIB_SM_MONITOR
 cps #MODE_MON
 bl monitor_reset
 cps #MODE_SVC
#endif
#if WITH_SMP
 /* figure out our cpu number */
 mrc     p15, 0, ip, c0, c0, 5 /* read MPIDR */

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

 /* if we're not cpu 0:0, jump back to arm_reset */
 cmp     ip, #0
 bne     arm_reset
#endif

.Lphys_offset:
 adr sp, .Lphys_offset /* sp = paddr */
 ldr ip, =.Lphys_offset /* ip = vaddr */
 sub ip, sp, ip  /* ip = phys_offset */

 /* patch mmu_initial_mappings table */
 ldr r5, =mmu_initial_mappings
 add r5, r5, ip /* r5 = _mmu_initial_mappings paddr */

.Lnext_entry:
 /* if size == 0, end of list */
 ldr r4, [r5, #__MMU_INITIAL_MAPPING_SIZE_OFFSET]
 cmp r4, #0
 beq .Lall_done

 ldr r4, [r5, #__MMU_INITIAL_MAPPING_FLAGS_OFFSET]
 tst r4, #MMU_INITIAL_MAPPING_FLAG_DYNAMIC
 addeq r5, #__MMU_INITIAL_MAPPING_SIZE
 beq .Lnext_entry

 /* patching dynamic entry: r5 - points to entry to patch */
 /* r0 is memsize passed in by the bootloader */

 /* update size field of mmu_initial_mappings struct */
 str r0, [r5, #__MMU_INITIAL_MAPPING_SIZE_OFFSET]

 /* calculate phys mem base */
 ldr r4, =_start /* r4 = _start vaddr */
 add     r4, r4, ip      /* r4 = _start paddr */

 /* update phys field of mmu_initial_mappings struct */
 str r4, [r5, #__MMU_INITIAL_MAPPING_PHYS_OFFSET]

.Lall_done:
 b arm_reset