arch/arm/cpu/armv7/psci.S

   1 /*
   2  * Copyright (C) 2013,2014 - ARM Ltd
   3  * Author: Marc Zyngier <marc.zyngier@arm.com>
   4  *
   5  * This program is free software; you can redistribute it and/or modify
   6  * it under the terms of the GNU General Public License version 2 as
   7  * published by the Free Software Foundation.
   8  *
   9  * This program is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <config.h>
  19 #include <linux/linkage.h>
  20 #include <asm/psci.h>
  21
  22         .pushsection ._secure.text, "ax"
  23
  24         .arch_extension sec
  25
  26         .align  5
  27         .globl _psci_vectors
  28 _psci_vectors:
  29         b       default_psci_vector     @ reset
  30         b       default_psci_vector     @ undef
  31         b       _smc_psci               @ smc
  32         b       default_psci_vector     @ pabort
  33         b       default_psci_vector     @ dabort
  34         b       default_psci_vector     @ hyp
  35         b       default_psci_vector     @ irq
  36         b       psci_fiq_enter          @ fiq
  37
  38 ENTRY(psci_fiq_enter)
  39         movs    pc, lr
  40 ENDPROC(psci_fiq_enter)
  41 .weak psci_fiq_enter
  42
  43 ENTRY(default_psci_vector)
  44         movs    pc, lr
  45 ENDPROC(default_psci_vector)
  46 .weak default_psci_vector
  47
  48 ENTRY(psci_cpu_suspend)
  49 ENTRY(psci_cpu_off)
  50 ENTRY(psci_cpu_on)
  51 ENTRY(psci_migrate)
  52         mov     r0, #ARM_PSCI_RET_NI    @ Return -1 (Not Implemented)
  53         mov     pc, lr
  54 ENDPROC(psci_migrate)
  55 ENDPROC(psci_cpu_on)
  56 ENDPROC(psci_cpu_off)
  57 ENDPROC(psci_cpu_suspend)
  58 .weak psci_cpu_suspend
  59 .weak psci_cpu_off
  60 .weak psci_cpu_on
  61 .weak psci_migrate
  62
  63 _psci_table:
  64         .word   ARM_PSCI_FN_CPU_SUSPEND
  65         .word   psci_cpu_suspend
  66         .word   ARM_PSCI_FN_CPU_OFF
  67         .word   psci_cpu_off
  68         .word   ARM_PSCI_FN_CPU_ON
  69         .word   psci_cpu_on
  70         .word   ARM_PSCI_FN_MIGRATE
  71         .word   psci_migrate
  72         .word   0
  73         .word   0
  74
  75 _smc_psci:
  76         push    {r4-r7,lr}
  77
  78         @ Switch to secure
  79         mrc     p15, 0, r7, c1, c1, 0
  80         bic     r4, r7, #1
  81         mcr     p15, 0, r4, c1, c1, 0
  82         isb
  83
  84         adr     r4, _psci_table
  85 1:      ldr     r5, [r4]                @ Load PSCI function ID
  86         ldr     r6, [r4, #4]            @ Load target PC
  87         cmp     r5, #0                  @ If reach the end, bail out
  88         moveq   r0, #ARM_PSCI_RET_INVAL @ Return -2 (Invalid)
  89         beq     2f
  90         cmp     r0, r5                  @ If not matching, try next entry
  91         addne   r4, r4, #8
  92         bne     1b
  93
  94         blx     r6                      @ Execute PSCI function
  95
  96         @ Switch back to non-secure
  97 2:      mcr     p15, 0, r7, c1, c1, 0
  98
  99         pop     {r4-r7, lr}
 100         movs    pc, lr                  @ Return to the kernel
 101
 102         .popsection