psci-suspend.S 1.2 KB

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