arch/arm/mach-imx/mx7/psci-suspend.S

   1 /* SPDX-License-Identifier: GPL-2.0+ */
   2 /*
   3  * Copyright 2018 NXP
   4  */
   5
   6 #include <config.h>
   7 #include <linux/linkage.h>
   8
   9 #include <asm/armv7.h>
  10 #include <asm/psci.h>
  11
  12         .pushsection ._secure.text, "ax"
  13
  14         .arch_extension sec
  15
  16 .globl v7_invalidate_l1
  17 v7_invalidate_l1:
  18         mov     r0, #0
  19         mcr     p15, 2, r0, c0, c0, 0
  20         mrc     p15, 1, r0, c0, c0, 0
  21
  22         movw    r1, #0x7fff
  23         and     r2, r1, r0, lsr #13
  24
  25         movw    r1, #0x3ff
  26
  27         and     r3, r1, r0, lsr #3      @ NumWays - 1
  28         add     r2, r2, #1              @ NumSets
  29
  30         and     r0, r0, #0x7
  31         add     r0, r0, #4      @ SetShift
  32
  33         clz     r1, r3          @ WayShift
  34         add     r4, r3, #1      @ NumWays
  35 1:
  36         sub     r2, r2, #1      @ NumSets--
  37         mov     r3, r4          @ Temp = NumWays
  38 2:
  39         subs    r3, r3, #1      @ Temp--
  40         mov     r5, r3, lsl r1
  41         mov     r6, r2, lsl r0
  42         orr     r5, r5, r6      @ Reg = (Temp<<WayShift)|(NumSets<<SetShift)
  43         mcr     p15, 0, r5, c7, c6, 2
  44         bgt     2b
  45         cmp     r2, #0
  46         bgt     1b
  47         dsb     st
  48         isb
  49         mov     pc, lr
  50
  51 .globl psci_system_resume
  52 psci_system_resume:
  53         mov     sp, r0
  54
  55         /* invalidate L1 I-cache first */
  56         mov     r6, #0x0
  57         mcr     p15, 0, r6, c7, c5, 0
  58         mcr     p15, 0, r6, c7, c5, 6
  59         /* enable the Icache and branch prediction */
  60         mov     r6, #0x1800
  61         mcr     p15, 0, r6, c1, c0, 0
  62         isb
  63
  64         bl      v7_invalidate_l1
  65         b       imx_system_resume
  66
  67         .popsection