Back to home page

LXR

 
 

    


0001 #include <linux/linkage.h>
0002 #include <linux/threads.h>
0003 #include <asm/asm-offsets.h>
0004 #include <asm/assembler.h>
0005 #include <asm/glue-cache.h>
0006 #include <asm/glue-proc.h>
0007     .text
0008 
0009 /*
0010  * Implementation of MPIDR hash algorithm through shifting
0011  * and OR'ing.
0012  *
0013  * @dst: register containing hash result
0014  * @rs0: register containing affinity level 0 bit shift
0015  * @rs1: register containing affinity level 1 bit shift
0016  * @rs2: register containing affinity level 2 bit shift
0017  * @mpidr: register containing MPIDR value
0018  * @mask: register containing MPIDR mask
0019  *
0020  * Pseudo C-code:
0021  *
0022  *u32 dst;
0023  *
0024  *compute_mpidr_hash(u32 rs0, u32 rs1, u32 rs2, u32 mpidr, u32 mask) {
0025  *  u32 aff0, aff1, aff2;
0026  *  u32 mpidr_masked = mpidr & mask;
0027  *  aff0 = mpidr_masked & 0xff;
0028  *  aff1 = mpidr_masked & 0xff00;
0029  *  aff2 = mpidr_masked & 0xff0000;
0030  *  dst = (aff0 >> rs0 | aff1 >> rs1 | aff2 >> rs2);
0031  *}
0032  * Input registers: rs0, rs1, rs2, mpidr, mask
0033  * Output register: dst
0034  * Note: input and output registers must be disjoint register sets
0035          (eg: a macro instance with mpidr = r1 and dst = r1 is invalid)
0036  */
0037     .macro compute_mpidr_hash dst, rs0, rs1, rs2, mpidr, mask
0038     and \mpidr, \mpidr, \mask           @ mask out MPIDR bits
0039     and \dst, \mpidr, #0xff         @ mask=aff0
0040  ARM(   mov \dst, \dst, lsr \rs0        )   @ dst=aff0>>rs0
0041  THUMB( lsr \dst, \dst, \rs0        )
0042     and \mask, \mpidr, #0xff00          @ mask = aff1
0043  ARM(   orr \dst, \dst, \mask, lsr \rs1 )   @ dst|=(aff1>>rs1)
0044  THUMB( lsr \mask, \mask, \rs1      )
0045  THUMB( orr \dst, \dst, \mask       )
0046     and \mask, \mpidr, #0xff0000        @ mask = aff2
0047  ARM(   orr \dst, \dst, \mask, lsr \rs2 )   @ dst|=(aff2>>rs2)
0048  THUMB( lsr \mask, \mask, \rs2      )
0049  THUMB( orr \dst, \dst, \mask       )
0050     .endm
0051 
0052 /*
0053  * Save CPU state for a suspend.  This saves the CPU general purpose
0054  * registers, and allocates space on the kernel stack to save the CPU
0055  * specific registers and some other data for resume.
0056  *  r0 = suspend function arg0
0057  *  r1 = suspend function
0058  *  r2 = MPIDR value the resuming CPU will use
0059  */
0060 ENTRY(__cpu_suspend)
0061     stmfd   sp!, {r4 - r11, lr}
0062 #ifdef MULTI_CPU
0063     ldr r10, =processor
0064     ldr r4, [r10, #CPU_SLEEP_SIZE] @ size of CPU sleep state
0065 #else
0066     ldr r4, =cpu_suspend_size
0067 #endif
0068     mov r5, sp          @ current virtual SP
0069     add r4, r4, #12     @ Space for pgd, virt sp, phys resume fn
0070     sub sp, sp, r4      @ allocate CPU state on stack
0071     ldr r3, =sleep_save_sp
0072     stmfd   sp!, {r0, r1}       @ save suspend func arg and pointer
0073     ldr r3, [r3, #SLEEP_SAVE_SP_VIRT]
0074     ALT_SMP(ldr r0, =mpidr_hash)
0075     ALT_UP_B(1f)
0076     /* This ldmia relies on the memory layout of the mpidr_hash struct */
0077     ldmia   r0, {r1, r6-r8} @ r1 = mpidr mask (r6,r7,r8) = l[0,1,2] shifts
0078     compute_mpidr_hash  r0, r6, r7, r8, r2, r1
0079     add r3, r3, r0, lsl #2
0080 1:  mov r2, r5          @ virtual SP
0081     mov r1, r4          @ size of save block
0082     add r0, sp, #8      @ pointer to save block
0083     bl  __cpu_suspend_save
0084     badr    lr, cpu_suspend_abort
0085     ldmfd   sp!, {r0, pc}       @ call suspend fn
0086 ENDPROC(__cpu_suspend)
0087     .ltorg
0088 
0089 cpu_suspend_abort:
0090     ldmia   sp!, {r1 - r3}      @ pop phys pgd, virt SP, phys resume fn
0091     teq r0, #0
0092     moveq   r0, #1          @ force non-zero value
0093     mov sp, r2
0094     ldmfd   sp!, {r4 - r11, pc}
0095 ENDPROC(cpu_suspend_abort)
0096 
0097 /*
0098  * r0 = control register value
0099  */
0100     .align  5
0101     .pushsection    .idmap.text,"ax"
0102 ENTRY(cpu_resume_mmu)
0103     ldr r3, =cpu_resume_after_mmu
0104     instr_sync
0105     mcr p15, 0, r0, c1, c0, 0   @ turn on MMU, I-cache, etc
0106     mrc p15, 0, r0, c0, c0, 0   @ read id reg
0107     instr_sync
0108     mov r0, r0
0109     mov r0, r0
0110     ret r3          @ jump to virtual address
0111 ENDPROC(cpu_resume_mmu)
0112     .popsection
0113 cpu_resume_after_mmu:
0114     bl  cpu_init        @ restore the und/abt/irq banked regs
0115     mov r0, #0          @ return zero on success
0116     ldmfd   sp!, {r4 - r11, pc}
0117 ENDPROC(cpu_resume_after_mmu)
0118 
0119     .text
0120     .align
0121 
0122 #ifdef CONFIG_MMU
0123     .arm
0124 ENTRY(cpu_resume_arm)
0125  THUMB( badr    r9, 1f      )   @ Kernel is entered in ARM.
0126  THUMB( bx  r9      )   @ If this is a Thumb-2 kernel,
0127  THUMB( .thumb          )   @ switch to Thumb now.
0128  THUMB(1:           )
0129 #endif
0130 
0131 ENTRY(cpu_resume)
0132 ARM_BE8(setend be)          @ ensure we are in BE mode
0133 #ifdef CONFIG_ARM_VIRT_EXT
0134     bl  __hyp_stub_install_secondary
0135 #endif
0136     safe_svcmode_maskall r1
0137     mov r1, #0
0138     ALT_SMP(mrc p15, 0, r0, c0, c0, 5)
0139     ALT_UP_B(1f)
0140     adr r2, mpidr_hash_ptr
0141     ldr r3, [r2]
0142     add r2, r2, r3      @ r2 = struct mpidr_hash phys address
0143     /*
0144      * This ldmia relies on the memory layout of the mpidr_hash
0145      * struct mpidr_hash.
0146      */
0147     ldmia   r2, { r3-r6 }   @ r3 = mpidr mask (r4,r5,r6) = l[0,1,2] shifts
0148     compute_mpidr_hash  r1, r4, r5, r6, r0, r3
0149 1:
0150     adr r0, _sleep_save_sp
0151     ldr r2, [r0]
0152     add r0, r0, r2
0153     ldr r0, [r0, #SLEEP_SAVE_SP_PHYS]
0154     ldr r0, [r0, r1, lsl #2]
0155 
0156     @ load phys pgd, stack, resume fn
0157   ARM(  ldmia   r0!, {r1, sp, pc}   )
0158 THUMB(  ldmia   r0!, {r1, r2, r3}   )
0159 THUMB(  mov sp, r2          )
0160 THUMB(  bx  r3          )
0161 ENDPROC(cpu_resume)
0162 
0163 #ifdef CONFIG_MMU
0164 ENDPROC(cpu_resume_arm)
0165 #endif
0166 
0167     .align 2
0168 _sleep_save_sp:
0169     .long   sleep_save_sp - .
0170 mpidr_hash_ptr:
0171     .long   mpidr_hash - .          @ mpidr_hash struct offset
0172 
0173     .data
0174     .type   sleep_save_sp, #object
0175 ENTRY(sleep_save_sp)
0176     .space  SLEEP_SAVE_SP_SZ        @ struct sleep_save_sp