plat/bcm/aarch32/opteed_helper.S

   1 /*
   2 <:copyright-BRCM:2019:DUAL/GPL:standard
   3
   4    Copyright (c) 2019 Broadcom
   5    All Rights Reserved
   6
   7 This program is free software; you can redistribute it and/or modify
   8 it under the terms of the GNU General Public License, version 2, as published by
   9 the Free Software Foundation (the "GPL").
  10
  11 This program is distributed in the hope that it will be useful,
  12 but WITHOUT ANY WARRANTY; without even the implied warranty of
  13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14 GNU General Public License for more details.
  15
  16
  17 A copy of the GPL is available at http://www.broadcom.com/licenses/GPLv2.php, or by
  18 writing to the Free Software Foundation, Inc., 59 Temple Place - Suite 330,
  19 Boston, MA 02111-1307, USA.
  20
  21 :>
  22 */
  23 #if defined(SPD_opteed)
  24 #include <asm_macros.S>
  25
  26 .global save_atf_sysreg
  27 .global restore_atf_sysreg
  28
  29     .macro read_sctlr reg
  30     mrc p15, 0, \reg, c1, c0, 0
  31     .endm
  32
  33     .macro write_sctlr reg
  34     mcr p15, 0, \reg, c1, c0, 0
  35     .endm
  36
  37     .macro read_scr reg
  38     mrc p15, 0, \reg, c1, c1, 0
  39     .endm
  40
  41     .macro write_scr reg
  42     mcr p15, 0, \reg, c1, c1, 0
  43     .endm
  44
  45     .macro write_ttbr0 reg
  46     mcr p15, 0, \reg, c2, c0, 0
  47     .endm
  48
  49     .macro read_ttbr0 reg
  50     mrc p15, 0, \reg, c2, c0, 0
  51     .endm
  52
  53     .macro write_ttbr1 reg
  54     mcr p15, 0, \reg, c2, c0, 1
  55     .endm
  56
  57     .macro read_ttbr1 reg
  58     mrc p15, 0, \reg, c2, c0, 1
  59     .endm
  60
  61     .macro write_ttbcr reg
  62     mcr p15, 0, \reg, c2, c0, 2
  63     .endm
  64
  65     .macro read_ttbcr reg
  66     mrc p15, 0, \reg, c2, c0, 2
  67     .endm
  68
  69     .macro write_dacr reg
  70     mcr p15, 0, \reg, c3, c0, 0
  71     .endm
  72
  73     .macro read_dacr reg
  74     mrc p15, 0, \reg, c3, c0, 0
  75     .endm
  76
  77     .macro read_vbar reg
  78     mrc p15, 0, \reg, c12, c0, 0
  79     .endm
  80
  81     .macro write_vbar reg
  82     mcr p15, 0, \reg, c12, c0, 0
  83     .endm
  84
  85     .macro write_mvbar reg
  86     mcr p15, 0, \reg, c12, c0, 1
  87     .endm
  88
  89     .macro read_mvbar reg
  90     mrc p15, 0, \reg, c12, c0, 1
  91     .endm
  92
  93     .macro write_mair0 reg
  94     mcr p15, 0, \reg, c10, c2, 0
  95     .endm
  96
  97     .macro read_mair0 reg
  98     mrc p15, 0, \reg, c10, c2, 0
  99     .endm
 100
 101     .macro write_mair1 reg
 102     mcr p15, 0, \reg, c10, c2, 1
 103     .endm
 104
 105     .macro read_mair1 reg
 106     mrc p15, 0, \reg, c10, c2, 1
 107     .endm
 108
 109     .macro read_cpsr reg
 110     mrs \reg, cpsr
 111     .endm
 112
 113     .macro write_cpsr reg
 114     msr cpsr_fsxc, \reg
 115     .endm
 116
 117    .macro invalidate_tlb reg
 118    mov \reg, #0
 119    mcr p15, 0, \reg, c8, c5, 0
 120    mcr p15, 0, \reg, c8, c6, 0
 121    mcr p15, 0, \reg, c8, c7, 0
 122    isb
 123    dsb
 124    .endm
 125
 126 /*
 127  *
 128  */
 129 func save_atf_sysreg
 130     push {r1, r4,r5}
 131
 132     mrc p15, 0, r1, c0, c0, 5
 133     and r1, r1, #0xff
 134     lsl r1, r1, #2
 135
 136     read_sctlr r4
 137     bic r4, r4, #0x5
 138     ldr r5, =atf_sctlr
 139     add r5, r5, r1
 140     str r4, [r5]
 141     read_cpsr r4
 142     ldr r5, =atf_cpsr
 143     add r5, r5, r1
 144     str r4, [r5]
 145
 146     read_scr r4
 147     ldr r5, =atf_scr
 148     str r4, [r5]
 149     read_vbar r4
 150     ldr r5, =atf_vbar
 151     str r4, [r5]
 152     read_mair0 r4
 153     ldr r5, =atf_mair0
 154     str r4, [r5]
 155     read_mair1 r4
 156     ldr r5, =atf_mair1
 157     str r4, [r5]
 158     read_dacr r4
 159     ldr r5, =atf_dacr
 160     str r4, [r5]
 161     read_ttbcr r4
 162     ldr r5, =atf_ttbcr
 163     str r4, [r5]
 164     read_mvbar r4
 165     ldr r5, =atf_mvbar
 166     str r4, [r5]
 167     read_ttbr0 r4
 168     ldr r5, =atf_ttbr0
 169     str r4, [r5]
 170     read_ttbr1 r4
 171     ldr r5, =atf_ttbr1
 172     str r4, [r5]
 173     pop {r1, r4,r5}
 174     bx lr
 175 endfunc save_atf_sysreg
 176
 177
 178 /*
 179  *
 180  */
 181 func restore_atf_sysreg
 182     push {r1,r9,r10}
 183
 184     mrc p15, 0, r1, c0, c0, 5
 185     and r1, r1, #0xff
 186     lsl r1, r1, #2
 187
 188     ldr r9, =atf_sctlr
 189     add r9, r9, r1
 190     ldr r10, [r9]
 191     write_sctlr r10
 192     ldr r9, =atf_cpsr
 193     add r9, r9, r1
 194     ldr r10, [r9]
 195     write_cpsr r10
 196
 197     ldr r9, =atf_scr
 198     ldr r10, [r9]
 199     write_scr r10
 200     ldr r9, =atf_vbar
 201     ldr r10, [r9]
 202     write_vbar r10
 203     ldr r9, =atf_mair0
 204     ldr r10, [r9]
 205     write_mair0 r10
 206     ldr r9, =atf_mair1
 207     ldr r10, [r9]
 208     write_mair1 r10
 209     ldr r9, =atf_dacr
 210     ldr r10, [r9]
 211     write_dacr r10
 212     ldr r9, =atf_ttbcr
 213     ldr r10, [r9]
 214     write_ttbcr r10
 215     ldr r9, =atf_mvbar
 216     ldr r10, [r9]
 217     write_mvbar r10
 218     ldr r9, =atf_ttbr0
 219     ldr r10, [r9]
 220     write_ttbr0 r10
 221     ldr r9, =atf_ttbr1
 222     ldr r10, [r9]
 223     write_ttbr1 r10
 224
 225     invalidate_tlb r10
 226
 227     pop {r1,r9,r10}
 228     bx lr
 229 endfunc restore_atf_sysreg
 230
 231
 232 .section .data
 233 atf_cpsr:
 234 .word 0x1234
 235 .word 0x1234
 236 .word 0x1234
 237 .word 0x1234
 238 atf_sctlr:
 239 .word 0x1234
 240 .word 0x1234
 241 .word 0x1234
 242 .word 0x1234
 243
 244 atf_scr:
 245 .word 0x1234
 246 atf_vbar:
 247 .word 0x1234
 248 atf_mair0:
 249 .word 0x1234
 250 atf_mair1:
 251 .word 0x1234
 252 atf_dacr:
 253 .word 0x1234
 254 atf_ttbcr:
 255 .word 0x1234
 256 atf_mvbar:
 257 .word 0x1234
 258 atf_ttbr0:
 259 .word 0x1234
 260 atf_ttbr1:
 261 .word 0x1234
 262
 263 #endif