sleep.S 4.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165
  1. #include <linux/linkage.h>
  2. #include <linux/threads.h>
  3. #include <asm/asm-offsets.h>
  4. #include <asm/assembler.h>
  5. #include <asm/glue-cache.h>
  6. #include <asm/glue-proc.h>
  7. .text
  8. /*
  9. * Implementation of MPIDR hash algorithm through shifting
  10. * and OR'ing.
  11. *
  12. * @dst: register containing hash result
  13. * @rs0: register containing affinity level 0 bit shift
  14. * @rs1: register containing affinity level 1 bit shift
  15. * @rs2: register containing affinity level 2 bit shift
  16. * @mpidr: register containing MPIDR value
  17. * @mask: register containing MPIDR mask
  18. *
  19. * Pseudo C-code:
  20. *
  21. *u32 dst;
  22. *
  23. *compute_mpidr_hash(u32 rs0, u32 rs1, u32 rs2, u32 mpidr, u32 mask) {
  24. * u32 aff0, aff1, aff2;
  25. * u32 mpidr_masked = mpidr & mask;
  26. * aff0 = mpidr_masked & 0xff;
  27. * aff1 = mpidr_masked & 0xff00;
  28. * aff2 = mpidr_masked & 0xff0000;
  29. * dst = (aff0 >> rs0 | aff1 >> rs1 | aff2 >> rs2);
  30. *}
  31. * Input registers: rs0, rs1, rs2, mpidr, mask
  32. * Output register: dst
  33. * Note: input and output registers must be disjoint register sets
  34. (eg: a macro instance with mpidr = r1 and dst = r1 is invalid)
  35. */
  36. .macro compute_mpidr_hash dst, rs0, rs1, rs2, mpidr, mask
  37. and \mpidr, \mpidr, \mask @ mask out MPIDR bits
  38. and \dst, \mpidr, #0xff @ mask=aff0
  39. ARM( mov \dst, \dst, lsr \rs0 ) @ dst=aff0>>rs0
  40. THUMB( lsr \dst, \dst, \rs0 )
  41. and \mask, \mpidr, #0xff00 @ mask = aff1
  42. ARM( orr \dst, \dst, \mask, lsr \rs1 ) @ dst|=(aff1>>rs1)
  43. THUMB( lsr \mask, \mask, \rs1 )
  44. THUMB( orr \dst, \dst, \mask )
  45. and \mask, \mpidr, #0xff0000 @ mask = aff2
  46. ARM( orr \dst, \dst, \mask, lsr \rs2 ) @ dst|=(aff2>>rs2)
  47. THUMB( lsr \mask, \mask, \rs2 )
  48. THUMB( orr \dst, \dst, \mask )
  49. .endm
  50. /*
  51. * Save CPU state for a suspend. This saves the CPU general purpose
  52. * registers, and allocates space on the kernel stack to save the CPU
  53. * specific registers and some other data for resume.
  54. * r0 = suspend function arg0
  55. * r1 = suspend function
  56. */
  57. ENTRY(__cpu_suspend)
  58. stmfd sp!, {r4 - r11, lr}
  59. #ifdef MULTI_CPU
  60. ldr r10, =processor
  61. ldr r4, [r10, #CPU_SLEEP_SIZE] @ size of CPU sleep state
  62. #else
  63. ldr r4, =cpu_suspend_size
  64. #endif
  65. mov r5, sp @ current virtual SP
  66. add r4, r4, #12 @ Space for pgd, virt sp, phys resume fn
  67. sub sp, sp, r4 @ allocate CPU state on stack
  68. stmfd sp!, {r0, r1} @ save suspend func arg and pointer
  69. add r0, sp, #8 @ save pointer to save block
  70. mov r1, r4 @ size of save block
  71. mov r2, r5 @ virtual SP
  72. ldr r3, =sleep_save_sp
  73. ldr r3, [r3, #SLEEP_SAVE_SP_VIRT]
  74. ALT_SMP(mrc p15, 0, r9, c0, c0, 5)
  75. ALT_UP_B(1f)
  76. ldr r8, =mpidr_hash
  77. /*
  78. * This ldmia relies on the memory layout of the mpidr_hash
  79. * struct mpidr_hash.
  80. */
  81. ldmia r8, {r4-r7} @ r4 = mpidr mask (r5,r6,r7) = l[0,1,2] shifts
  82. compute_mpidr_hash lr, r5, r6, r7, r9, r4
  83. add r3, r3, lr, lsl #2
  84. 1:
  85. bl __cpu_suspend_save
  86. adr lr, BSYM(cpu_suspend_abort)
  87. ldmfd sp!, {r0, pc} @ call suspend fn
  88. ENDPROC(__cpu_suspend)
  89. .ltorg
  90. cpu_suspend_abort:
  91. ldmia sp!, {r1 - r3} @ pop phys pgd, virt SP, phys resume fn
  92. teq r0, #0
  93. moveq r0, #1 @ force non-zero value
  94. mov sp, r2
  95. ldmfd sp!, {r4 - r11, pc}
  96. ENDPROC(cpu_suspend_abort)
  97. /*
  98. * r0 = control register value
  99. */
  100. .align 5
  101. .pushsection .idmap.text,"ax"
  102. ENTRY(cpu_resume_mmu)
  103. ldr r3, =cpu_resume_after_mmu
  104. instr_sync
  105. mcr p15, 0, r0, c1, c0, 0 @ turn on MMU, I-cache, etc
  106. mrc p15, 0, r0, c0, c0, 0 @ read id reg
  107. instr_sync
  108. mov r0, r0
  109. mov r0, r0
  110. mov pc, r3 @ jump to virtual address
  111. ENDPROC(cpu_resume_mmu)
  112. .popsection
  113. cpu_resume_after_mmu:
  114. bl cpu_init @ restore the und/abt/irq banked regs
  115. mov r0, #0 @ return zero on success
  116. ldmfd sp!, {r4 - r11, pc}
  117. ENDPROC(cpu_resume_after_mmu)
  118. /*
  119. * Note: Yes, part of the following code is located into the .data section.
  120. * This is to allow sleep_save_sp to be accessed with a relative load
  121. * while we can't rely on any MMU translation. We could have put
  122. * sleep_save_sp in the .text section as well, but some setups might
  123. * insist on it to be truly read-only.
  124. */
  125. .data
  126. .align
  127. ENTRY(cpu_resume)
  128. mov r1, #0
  129. ALT_SMP(mrc p15, 0, r0, c0, c0, 5)
  130. ALT_UP_B(1f)
  131. adr r2, mpidr_hash_ptr
  132. ldr r3, [r2]
  133. add r2, r2, r3 @ r2 = struct mpidr_hash phys address
  134. /*
  135. * This ldmia relies on the memory layout of the mpidr_hash
  136. * struct mpidr_hash.
  137. */
  138. ldmia r2, { r3-r6 } @ r3 = mpidr mask (r4,r5,r6) = l[0,1,2] shifts
  139. compute_mpidr_hash r1, r4, r5, r6, r0, r3
  140. 1:
  141. adr r0, _sleep_save_sp
  142. ldr r0, [r0, #SLEEP_SAVE_SP_PHYS]
  143. ldr r0, [r0, r1, lsl #2]
  144. setmode PSR_I_BIT | PSR_F_BIT | SVC_MODE, r1 @ set SVC, irqs off
  145. @ load phys pgd, stack, resume fn
  146. ARM( ldmia r0!, {r1, sp, pc} )
  147. THUMB( ldmia r0!, {r1, r2, r3} )
  148. THUMB( mov sp, r2 )
  149. THUMB( bx r3 )
  150. ENDPROC(cpu_resume)
  151. .align 2
  152. mpidr_hash_ptr:
  153. .long mpidr_hash - . @ mpidr_hash struct offset
  154. .type sleep_save_sp, #object
  155. ENTRY(sleep_save_sp)
  156. _sleep_save_sp:
  157. .space SLEEP_SAVE_SP_SZ @ struct sleep_save_sp