diff options
author | Andrew Thoelke <andrew.thoelke@arm.com> | 2014-05-23 11:00:04 +0100 |
---|---|---|
committer | Andrew Thoelke <andrew.thoelke@arm.com> | 2014-05-23 11:00:04 +0100 |
commit | 8545a8744b541cc6855e3218c4565e76697fb002 (patch) | |
tree | 96fdd4f46996c69897634eabd1e517ab83013f24 /bl32 | |
parent | 92535302791564e102150072d0e6152f9c4fde87 (diff) | |
parent | a20a81e5b4a19969673f672523b946647f5d545d (diff) |
Merge pull request #102 from achingupta:ag/tf-issues#104-v2
Diffstat (limited to 'bl32')
-rw-r--r-- | bl32/tsp/aarch64/tsp_entrypoint.S | 73 | ||||
-rw-r--r-- | bl32/tsp/aarch64/tsp_exceptions.S | 200 | ||||
-rw-r--r-- | bl32/tsp/tsp-fvp.mk | 8 | ||||
-rw-r--r-- | bl32/tsp/tsp.mk | 3 | ||||
-rw-r--r-- | bl32/tsp/tsp_interrupt.c | 109 | ||||
-rw-r--r-- | bl32/tsp/tsp_main.c | 24 | ||||
-rw-r--r-- | bl32/tsp/tsp_timer.c | 106 |
7 files changed, 517 insertions, 6 deletions
diff --git a/bl32/tsp/aarch64/tsp_entrypoint.S b/bl32/tsp/aarch64/tsp_entrypoint.S index fab64cf5..54276f2c 100644 --- a/bl32/tsp/aarch64/tsp_entrypoint.S +++ b/bl32/tsp/aarch64/tsp_entrypoint.S @@ -39,6 +39,7 @@ .globl tsp_cpu_suspend_entry .globl tsp_cpu_resume_entry .globl tsp_fast_smc_entry + .globl tsp_fiq_entry /* --------------------------------------------- * Populate the params in x0-x7 from the pointer @@ -53,6 +54,22 @@ smc #0 .endm + .macro save_eret_context reg1 reg2 + mrs \reg1, elr_el1 + mrs \reg2, spsr_el1 + stp \reg1, \reg2, [sp, #-0x10]! + stp x30, x18, [sp, #-0x10]! + .endm + + .macro restore_eret_context reg1 reg2 + ldp x30, x18, [sp], #0x10 + ldp \reg1, \reg2, [sp], #0x10 + msr elr_el1, \reg1 + msr spsr_el1, \reg2 + .endm + + .section .text, "ax" + .align 3 func tsp_entrypoint @@ -70,7 +87,7 @@ func tsp_entrypoint * Set the exception vector to something sane. * --------------------------------------------- */ - adr x0, early_exceptions + adr x0, tsp_exceptions msr vbar_el1, x0 /* --------------------------------------------- @@ -167,7 +184,7 @@ func tsp_cpu_on_entry * Set the exception vector to something sane. * --------------------------------------------- */ - adr x0, early_exceptions + adr x0, tsp_exceptions msr vbar_el1, x0 /* --------------------------------------------- @@ -227,6 +244,58 @@ func tsp_cpu_suspend_entry restore_args_call_smc /*--------------------------------------------- + * This entrypoint is used by the TSPD to pass + * control for handling a pending S-EL1 FIQ. + * 'x0' contains a magic number which indicates + * this. TSPD expects control to be handed back + * at the end of FIQ processing. This is done + * through an SMC. The handover agreement is: + * + * 1. PSTATE.DAIF are set upon entry. 'x1' has + * the ELR_EL3 from the non-secure state. + * 2. TSP has to preserve the callee saved + * general purpose registers, SP_EL1/EL0 and + * LR. + * 3. TSP has to preserve the system and vfp + * registers (if applicable). + * 4. TSP can use 'x0-x18' to enable its C + * runtime. + * 5. TSP returns to TSPD using an SMC with + * 'x0' = TSP_HANDLED_S_EL1_FIQ + * --------------------------------------------- + */ +func tsp_fiq_entry +#if DEBUG + mov x2, #(TSP_HANDLE_FIQ_AND_RETURN & ~0xffff) + movk x2, #(TSP_HANDLE_FIQ_AND_RETURN & 0xffff) + cmp x0, x2 + b.ne tsp_fiq_entry_panic +#endif + /*--------------------------------------------- + * Save any previous context needed to perform + * an exception return from S-EL1 e.g. context + * from a previous IRQ. Update statistics and + * handle the FIQ before returning to the TSPD. + * IRQ/FIQs are not enabled since that will + * complicate the implementation. Execution + * will be transferred back to the normal world + * in any case. A non-zero return value from the + * fiq handler is an error. + * --------------------------------------------- + */ + save_eret_context x2 x3 + bl tsp_update_sync_fiq_stats + bl tsp_fiq_handler + cbnz x0, tsp_fiq_entry_panic + restore_eret_context x2 x3 + mov x0, #(TSP_HANDLED_S_EL1_FIQ & ~0xffff) + movk x0, #(TSP_HANDLED_S_EL1_FIQ & 0xffff) + smc #0 + +tsp_fiq_entry_panic: + b tsp_fiq_entry_panic + + /*--------------------------------------------- * This entrypoint is used by the TSPD when this * cpu resumes execution after an earlier * CPU_SUSPEND psci call to ask the TSP to diff --git a/bl32/tsp/aarch64/tsp_exceptions.S b/bl32/tsp/aarch64/tsp_exceptions.S new file mode 100644 index 00000000..ccb4cddd --- /dev/null +++ b/bl32/tsp/aarch64/tsp_exceptions.S @@ -0,0 +1,200 @@ +/* + * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * Neither the name of ARM nor the names of its contributors may be used + * to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include <bl_common.h> +#include <arch.h> +#include <tsp.h> +#include <asm_macros.S> + + + /* ---------------------------------------------------- + * The caller-saved registers x0-x18 and LR are saved + * here. + * ---------------------------------------------------- + */ + +#define SCRATCH_REG_SIZE #(20 * 8) + + .macro save_caller_regs_and_lr + sub sp, sp, SCRATCH_REG_SIZE + stp x0, x1, [sp] + stp x2, x3, [sp, #0x10] + stp x4, x5, [sp, #0x20] + stp x6, x7, [sp, #0x30] + stp x8, x9, [sp, #0x40] + stp x10, x11, [sp, #0x50] + stp x12, x13, [sp, #0x60] + stp x14, x15, [sp, #0x70] + stp x16, x17, [sp, #0x80] + stp x18, x30, [sp, #0x90] + .endm + + .macro restore_caller_regs_and_lr + ldp x0, x1, [sp] + ldp x2, x3, [sp, #0x10] + ldp x4, x5, [sp, #0x20] + ldp x6, x7, [sp, #0x30] + ldp x8, x9, [sp, #0x40] + ldp x10, x11, [sp, #0x50] + ldp x12, x13, [sp, #0x60] + ldp x14, x15, [sp, #0x70] + ldp x16, x17, [sp, #0x80] + ldp x18, x30, [sp, #0x90] + add sp, sp, SCRATCH_REG_SIZE + .endm + + .globl tsp_exceptions + + /* ----------------------------------------------------- + * TSP exception handlers. + * ----------------------------------------------------- + */ + .section .vectors, "ax"; .align 11 + + .align 7 +tsp_exceptions: + /* ----------------------------------------------------- + * Current EL with _sp_el0 : 0x0 - 0x180. No exceptions + * are expected and treated as irrecoverable errors. + * ----------------------------------------------------- + */ +sync_exception_sp_el0: + wfi + b sync_exception_sp_el0 + check_vector_size sync_exception_sp_el0 + + .align 7 + +irq_sp_el0: + b irq_sp_el0 + check_vector_size irq_sp_el0 + + .align 7 +fiq_sp_el0: + b fiq_sp_el0 + check_vector_size fiq_sp_el0 + + .align 7 +serror_sp_el0: + b serror_sp_el0 + check_vector_size serror_sp_el0 + + + /* ----------------------------------------------------- + * Current EL with SPx: 0x200 - 0x380. Only IRQs/FIQs + * are expected and handled + * ----------------------------------------------------- + */ + .align 7 +sync_exception_sp_elx: + wfi + b sync_exception_sp_elx + check_vector_size sync_exception_sp_elx + + .align 7 +irq_sp_elx: + b irq_sp_elx + check_vector_size irq_sp_elx + + .align 7 +fiq_sp_elx: + save_caller_regs_and_lr + bl tsp_fiq_handler + cbz x0, fiq_sp_elx_done + + /* + * This FIQ was not targetted to S-EL1 so send it to + * the monitor and wait for execution to resume. + */ + smc #0 +fiq_sp_elx_done: + restore_caller_regs_and_lr + eret + check_vector_size fiq_sp_elx + + .align 7 +serror_sp_elx: + b serror_sp_elx + check_vector_size serror_sp_elx + + + /* ----------------------------------------------------- + * Lower EL using AArch64 : 0x400 - 0x580. No exceptions + * are handled since TSP does not implement a lower EL + * ----------------------------------------------------- + */ + .align 7 +sync_exception_aarch64: + wfi + b sync_exception_aarch64 + check_vector_size sync_exception_aarch64 + + .align 7 +irq_aarch64: + b irq_aarch64 + check_vector_size irq_aarch64 + + .align 7 +fiq_aarch64: + b fiq_aarch64 + check_vector_size fiq_aarch64 + + .align 7 +serror_aarch64: + b serror_aarch64 + check_vector_size serror_aarch64 + + + /* ----------------------------------------------------- + * Lower EL using AArch32 : 0x600 - 0x780. No exceptions + * handled since the TSP does not implement a lower EL. + * ----------------------------------------------------- + */ + .align 7 +sync_exception_aarch32: + wfi + b sync_exception_aarch32 + check_vector_size sync_exception_aarch32 + + .align 7 +irq_aarch32: + b irq_aarch32 + check_vector_size irq_aarch32 + + .align 7 +fiq_aarch32: + b fiq_aarch32 + check_vector_size fiq_aarch32 + + .align 7 +serror_aarch32: + b serror_aarch32 + check_vector_size serror_aarch32 + .align 7 diff --git a/bl32/tsp/tsp-fvp.mk b/bl32/tsp/tsp-fvp.mk index 5d8a0e34..b1d0afef 100644 --- a/bl32/tsp/tsp-fvp.mk +++ b/bl32/tsp/tsp-fvp.mk @@ -29,7 +29,9 @@ # # TSP source files specific to FVP platform -BL32_SOURCES += plat/common/aarch64/platform_mp_stack.S \ - plat/fvp/bl32_plat_setup.c \ +BL32_SOURCES += drivers/arm/gic/gic_v2.c \ + plat/common/aarch64/platform_mp_stack.S \ plat/fvp/aarch64/plat_common.c \ - plat/fvp/aarch64/plat_helpers.S + plat/fvp/aarch64/plat_helpers.S \ + plat/fvp/bl32_plat_setup.c \ + plat/fvp/plat_gic.c diff --git a/bl32/tsp/tsp.mk b/bl32/tsp/tsp.mk index c478b435..b9084d54 100644 --- a/bl32/tsp/tsp.mk +++ b/bl32/tsp/tsp.mk @@ -30,7 +30,10 @@ BL32_SOURCES += bl32/tsp/tsp_main.c \ bl32/tsp/aarch64/tsp_entrypoint.S \ + bl32/tsp/aarch64/tsp_exceptions.S \ bl32/tsp/aarch64/tsp_request.S \ + bl32/tsp/tsp_interrupt.c \ + bl32/tsp/tsp_timer.c \ common/aarch64/early_exceptions.S \ lib/locks/exclusive/spinlock.S diff --git a/bl32/tsp/tsp_interrupt.c b/bl32/tsp/tsp_interrupt.c new file mode 100644 index 00000000..d5d02c30 --- /dev/null +++ b/bl32/tsp/tsp_interrupt.c @@ -0,0 +1,109 @@ +/* + * Copyright (c) 2014, ARM Limited and Contributors. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * Neither the name of ARM nor the names of its contributors may be used + * to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include <arch_helpers.h> +#include <assert.h> +#include <debug.h> +#include <gic_v2.h> +#include <tsp.h> +#include <platform.h> + +/******************************************************************************* + * This function updates the TSP statistics for FIQs handled synchronously i.e + * the ones that have been handed over by the TSPD. It also keeps count of the + * number of times control was passed back to the TSPD after handling an FIQ. + * In the future it will be possible that the TSPD hands over an FIQ to the TSP + * but does not expect it to return execution. This statistic will be useful to + * distinguish between these two models of synchronous FIQ handling. + * The 'elr_el3' parameter contains the address of the instruction in normal + * world where this FIQ was generated. + ******************************************************************************/ +void tsp_update_sync_fiq_stats(uint32_t type, uint64_t elr_el3) +{ + uint64_t mpidr = read_mpidr(); + uint32_t linear_id = platform_get_core_pos(mpidr); + + tsp_stats[linear_id].sync_fiq_count++; + if (type == TSP_HANDLE_FIQ_AND_RETURN) + tsp_stats[linear_id].sync_fiq_ret_count++; + + spin_lock(&console_lock); + printf("TSP: cpu 0x%x sync fiq request from 0x%llx \n\r", + mpidr, elr_el3); + INFO("cpu 0x%x: %d sync fiq requests, %d sync fiq returns\n", + mpidr, + tsp_stats[linear_id].sync_fiq_count, + tsp_stats[linear_id].sync_fiq_ret_count); + spin_unlock(&console_lock); +} + +/******************************************************************************* + * TSP FIQ handler called as a part of both synchronous and asynchronous + * handling of FIQ interrupts. It returns 0 upon successfully handling a S-EL1 + * FIQ and treats all other FIQs as EL3 interrupts. It assumes that the GIC + * architecture version in v2.0 and the secure physical timer interrupt is the + * only S-EL1 interrupt that it needs to handle. + ******************************************************************************/ +int32_t tsp_fiq_handler() +{ + uint64_t mpidr = read_mpidr(); + uint32_t linear_id = platform_get_core_pos(mpidr), id; + + /* + * Get the highest priority pending interrupt id and see if it is the + * secure physical generic timer interrupt in which case, handle it. + * Otherwise throw this interrupt at the EL3 firmware. + */ + id = ic_get_pending_interrupt_id(); + + /* TSP can only handle the secure physical timer interrupt */ + if (id != IRQ_SEC_PHY_TIMER) + return TSP_EL3_FIQ; + + /* + * Handle the interrupt. Also sanity check if it has been preempted by + * another secure interrupt through an assertion. + */ + id = ic_acknowledge_interrupt(); + assert(id == IRQ_SEC_PHY_TIMER); + tsp_generic_timer_handler(); + ic_end_of_interrupt(id); + + /* Update the statistics and print some messages */ + tsp_stats[linear_id].fiq_count++; + spin_lock(&console_lock); + printf("TSP: cpu 0x%x handled fiq %d \n\r", + mpidr, id); + INFO("cpu 0x%x: %d fiq requests \n", + mpidr, tsp_stats[linear_id].fiq_count); + spin_unlock(&console_lock); + + return 0; +} diff --git a/bl32/tsp/tsp_main.c b/bl32/tsp/tsp_main.c index a7c73865..ec74db42 100644 --- a/bl32/tsp/tsp_main.c +++ b/bl32/tsp/tsp_main.c @@ -58,7 +58,7 @@ static tsp_args_t tsp_smc_args[PLATFORM_CORE_COUNT]; /******************************************************************************* * Per cpu data structure to keep track of TSP activity ******************************************************************************/ -static work_statistics_t tsp_stats[PLATFORM_CORE_COUNT]; +work_statistics_t tsp_stats[PLATFORM_CORE_COUNT]; /******************************************************************************* * Single reference to the various entry points exported by the test secure @@ -71,6 +71,7 @@ static const entry_info_t tsp_entry_info = { tsp_cpu_off_entry, tsp_cpu_resume_entry, tsp_cpu_suspend_entry, + tsp_fiq_entry, }; @@ -127,6 +128,7 @@ uint64_t tsp_main(void) bl32_platform_setup(); /* Initialize secure/applications state here */ + tsp_generic_timer_start(); /* Update this cpu's statistics */ tsp_stats[linear_id].smc_count++; @@ -162,6 +164,9 @@ tsp_args_t *tsp_cpu_on_main(void) uint64_t mpidr = read_mpidr(); uint32_t linear_id = platform_get_core_pos(mpidr); + /* Initialize secure/applications state here */ + tsp_generic_timer_start(); + /* Update this cpu's statistics */ tsp_stats[linear_id].smc_count++; tsp_stats[linear_id].eret_count++; @@ -195,6 +200,13 @@ tsp_args_t *tsp_cpu_off_main(uint64_t arg0, uint64_t mpidr = read_mpidr(); uint32_t linear_id = platform_get_core_pos(mpidr); + /* + * This cpu is being turned off, so disable the timer to prevent the + * secure timer interrupt from interfering with power down. A pending + * interrupt will be lost but we do not care as we are turning off. + */ + tsp_generic_timer_stop(); + /* Update this cpu's statistics */ tsp_stats[linear_id].smc_count++; tsp_stats[linear_id].eret_count++; @@ -230,6 +242,13 @@ tsp_args_t *tsp_cpu_suspend_main(uint64_t power_state, uint64_t mpidr = read_mpidr(); uint32_t linear_id = platform_get_core_pos(mpidr); + /* + * Save the time context and disable it to prevent the secure timer + * interrupt from interfering with wakeup from the suspend state. + */ + tsp_generic_timer_save(); + tsp_generic_timer_stop(); + /* Update this cpu's statistics */ tsp_stats[linear_id].smc_count++; tsp_stats[linear_id].eret_count++; @@ -265,6 +284,9 @@ tsp_args_t *tsp_cpu_resume_main(uint64_t suspend_level, uint64_t mpidr = read_mpidr(); uint32_t linear_id = platform_get_core_pos(mpidr); + /* Restore the generic timer context */ + tsp_generic_timer_restore(); + /* Update this cpu's statistics */ tsp_stats[linear_id].smc_count++; tsp_stats[linear_id].eret_count++; diff --git a/bl32/tsp/tsp_timer.c b/bl32/tsp/tsp_timer.c new file mode 100644 index 00000000..f66ff9fa --- /dev/null +++ b/bl32/tsp/tsp_timer.c @@ -0,0 +1,106 @@ +/* + * Copyright (c) 2014, ARM Limited and Contributors. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * Neither the name of ARM nor the names of its contributors may be used + * to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include <arch_helpers.h> +#include <assert.h> +#include <tsp.h> + +/******************************************************************************* + * Data structure to keep track of per-cpu secure generic timer context across + * power management operations. + ******************************************************************************/ +typedef struct timer_context { + uint64_t cval; + uint32_t ctl; +} timer_context_t; + +static timer_context_t pcpu_timer_context[PLATFORM_CORE_COUNT]; + +/******************************************************************************* + * This function initializes the generic timer to fire every 0.5 second + ******************************************************************************/ +void tsp_generic_timer_start() +{ + uint64_t cval; + uint32_t ctl = 0; + + /* The timer will fire every 0.5 second */ + cval = read_cntpct_el0() + (read_cntfrq_el0() >> 1); + write_cntps_cval_el1(cval); + + /* Enable the secure physical timer */ + set_cntp_ctl_enable(ctl); + write_cntps_ctl_el1(ctl); +} + +/******************************************************************************* + * This function deasserts the timer interrupt and sets it up again + ******************************************************************************/ +void tsp_generic_timer_handler() +{ + /* Ensure that the timer did assert the interrupt */ + assert(get_cntp_ctl_istatus(read_cntps_ctl_el1())); + + /* Disable the timer and reprogram it */ + write_cntps_ctl_el1(0); + tsp_generic_timer_start(); +} + +/******************************************************************************* + * This function deasserts the timer interrupt prior to cpu power down + ******************************************************************************/ +void tsp_generic_timer_stop() +{ + /* Disable the timer */ + write_cntps_ctl_el1(0); +} + +/******************************************************************************* + * This function saves the timer context prior to cpu suspension + ******************************************************************************/ +void tsp_generic_timer_save() +{ + uint32_t linear_id = platform_get_core_pos(read_mpidr()); + + pcpu_timer_context[linear_id].cval = read_cntps_cval_el1(); + pcpu_timer_context[linear_id].ctl = read_cntps_ctl_el1(); + flush_dcache_range((uint64_t) &pcpu_timer_context[linear_id], + sizeof(pcpu_timer_context[linear_id])); +} + +/******************************************************************************* + * This function restores the timer context post cpu resummption + ******************************************************************************/ +void tsp_generic_timer_restore() +{ + uint32_t linear_id = platform_get_core_pos(read_mpidr()); + + write_cntps_cval_el1(pcpu_timer_context[linear_id].cval); + write_cntps_ctl_el1(pcpu_timer_context[linear_id].ctl); +} |