psci.S 2.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110
  1. /* SPDX-License-Identifier: GPL-2.0+ */
  2. /*
  3. * Copyright (C) 2014, NVIDIA
  4. * Copyright (C) 2015, Siemens AG
  5. *
  6. * Authors:
  7. * Thierry Reding <treding@nvidia.com>
  8. * Jan Kiszka <jan.kiszka@siemens.com>
  9. */
  10. #include <linux/linkage.h>
  11. #include <asm/macro.h>
  12. #include <asm/psci.h>
  13. .pushsection ._secure.text, "ax"
  14. .arch_extension sec
  15. #define TEGRA_SB_CSR_0 0x6000c200
  16. #define NS_RST_VEC_WR_DIS (1 << 1)
  17. #define TEGRA_RESET_EXCEPTION_VECTOR 0x6000f100
  18. #define TEGRA_FLOW_CTRL_BASE 0x60007000
  19. #define FLOW_CTRL_CPU_CSR 0x08
  20. #define CSR_ENABLE (1 << 0)
  21. #define CSR_IMMEDIATE_WAKE (1 << 3)
  22. #define CSR_WAIT_WFI_SHIFT 8
  23. #define FLOW_CTRL_CPU1_CSR 0x18
  24. @ converts CPU ID into FLOW_CTRL_CPUn_CSR offset
  25. .macro get_csr_reg cpu, ofs, tmp
  26. cmp \cpu, #0 @ CPU0?
  27. lsl \tmp, \cpu, #3 @ multiple by 8 (register offset CPU1-3)
  28. moveq \ofs, #FLOW_CTRL_CPU_CSR
  29. addne \ofs, \tmp, #FLOW_CTRL_CPU1_CSR - 8
  30. .endm
  31. ENTRY(psci_arch_init)
  32. mov r6, lr
  33. mrc p15, 0, r5, c1, c1, 0 @ Read SCR
  34. bic r5, r5, #1 @ Secure mode
  35. mcr p15, 0, r5, c1, c1, 0 @ Write SCR
  36. isb
  37. @ lock reset vector for non-secure
  38. ldr r4, =TEGRA_SB_CSR_0
  39. ldr r5, [r4]
  40. orr r5, r5, #NS_RST_VEC_WR_DIS
  41. str r5, [r4]
  42. bl psci_get_cpu_id @ CPU ID => r0
  43. adr r5, _sys_clock_freq
  44. cmp r0, #0
  45. mrceq p15, 0, r7, c14, c0, 0 @ read CNTFRQ from CPU0
  46. streq r7, [r5]
  47. ldrne r7, [r5]
  48. mcrne p15, 0, r7, c14, c0, 0 @ write CNTFRQ to CPU1..3
  49. bx r6
  50. ENDPROC(psci_arch_init)
  51. _sys_clock_freq:
  52. .word 0
  53. ENTRY(psci_cpu_off)
  54. bl psci_cpu_off_common
  55. bl psci_get_cpu_id @ CPU ID => r0
  56. get_csr_reg r0, r2, r3
  57. ldr r6, =TEGRA_FLOW_CTRL_BASE
  58. mov r5, #(CSR_ENABLE)
  59. mov r4, #(1 << CSR_WAIT_WFI_SHIFT)
  60. add r5, r4, lsl r0
  61. str r5, [r6, r2]
  62. _loop: wfi
  63. b _loop
  64. ENDPROC(psci_cpu_off)
  65. ENTRY(psci_cpu_on)
  66. push {r4, r5, r6, lr}
  67. mov r4, r1
  68. mov r0, r1
  69. mov r1, r2
  70. mov r2, r3
  71. bl psci_save @ store target PC and context id
  72. mov r1, r4
  73. ldr r6, =TEGRA_RESET_EXCEPTION_VECTOR
  74. ldr r5, =psci_cpu_entry
  75. str r5, [r6]
  76. get_csr_reg r1, r2, r3
  77. ldr r6, =TEGRA_FLOW_CTRL_BASE
  78. mov r5, #(CSR_IMMEDIATE_WAKE | CSR_ENABLE)
  79. str r5, [r6, r2]
  80. mov r0, #ARM_PSCI_RET_SUCCESS @ Return PSCI_RET_SUCCESS
  81. pop {r4, r5, r6, pc}
  82. ENDPROC(psci_cpu_on)
  83. .popsection