arch/arm/mach-mt8127/swsusp.S

   1 /*
   2  * swsusp.S - This file is based on arch/i386/power/swsusp.S;
   3  *
   4  *  This program is free software; you can redistribute it and/or modify
   5  *  it under the terms of the GNU General Public License as published by
   6  *  the Free Software Foundation; either version 2 of the License.
   7  *
   8  *  This program is distributed in the hope that it will be useful,
   9  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  10  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11  *  GNU General Public License for more details.
  12  *
  13  *  You should have received a copy of the GNU General Public License
  14  *  along with this program; if not, write to the Free Software
  15  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  16  */
  17
  18 /*
  19  * This may not use any stack, nor any variable that is not "NoSave":
  20  *
  21  * Its rewriting one kernel image with another. What is stack in "old"
  22  * image could very well be data page in "new" image, and overwriting
  23  * your own stack under you is bad idea.
  24  */
  25
  26 /*
  27  * FIXME: Work needs to be done for core with fp.
  28  */
  29
  30 #include <linux/linkage.h>
  31 #include <asm/memory.h>
  32 #include <asm/segment.h>
  33 #include <asm/page.h>
  34 #include <asm/asm-offsets.h>
  35
  36         .text
  37
  38 #define LOCAL_WORD(x) \
  39         .data                   ; \
  40         .p2align 2              ; \
  41         .type   x, #object      ; \
  42         .size   x, 4            ; \
  43 x:                              ; \
  44         .long 1
  45
  46 #define WORD_ADDR(x) \
  47         .align 2                ; \
  48 .L##x:                          ; \
  49         .word x
  50
  51 #define FUNC(x) \
  52         .text                   ; \
  53         .p2align 2              ; \
  54         .globl x                ; \
  55         .type   x, #function    ; \
  56 x:
  57
  58 #define FUNC_END(x) \
  59         .size   x, .-x
  60
  61 #define CHANGE_MODE(x) \
  62         mov     r1, r0          ; \
  63         bic     r1, r1, #0x1f   ; \
  64         orr     r1, r1, #0x##x  ; \
  65         msr     cpsr_c, r1
  66
  67 /* nonvolatile int registers */
  68 #ifdef DEBUG
  69         .globl  saved_context_r0        // for debug
  70 #endif
  71         LOCAL_WORD(saved_context_r0)
  72         LOCAL_WORD(saved_context_r1)
  73         LOCAL_WORD(saved_context_r2)
  74         LOCAL_WORD(saved_context_r3)
  75         LOCAL_WORD(saved_context_r4)
  76         LOCAL_WORD(saved_context_r5)
  77         LOCAL_WORD(saved_context_r6)
  78         LOCAL_WORD(saved_context_r7)
  79         LOCAL_WORD(saved_context_r8)
  80         LOCAL_WORD(saved_context_r9)
  81         LOCAL_WORD(saved_context_r10)
  82         LOCAL_WORD(saved_context_r11)
  83         LOCAL_WORD(saved_context_r12)
  84         LOCAL_WORD(saved_context_r13)
  85         LOCAL_WORD(saved_context_r14)
  86         LOCAL_WORD(saved_cpsr)
  87
  88         LOCAL_WORD(saved_context_r8_fiq)
  89         LOCAL_WORD(saved_context_r9_fiq)
  90         LOCAL_WORD(saved_context_r10_fiq)
  91         LOCAL_WORD(saved_context_r11_fiq)
  92         LOCAL_WORD(saved_context_r12_fiq)
  93         LOCAL_WORD(saved_context_r13_fiq)
  94         LOCAL_WORD(saved_context_r14_fiq)
  95         LOCAL_WORD(saved_spsr_fiq)
  96
  97         LOCAL_WORD(saved_context_r13_irq)
  98         LOCAL_WORD(saved_context_r14_irq)
  99         LOCAL_WORD(saved_spsr_irq)
 100         LOCAL_WORD(saved_context_r13_svc)
 101         LOCAL_WORD(saved_context_r14_svc)
 102         LOCAL_WORD(saved_spsr_svc)
 103
 104         LOCAL_WORD(saved_context_r13_abt)
 105         LOCAL_WORD(saved_context_r14_abt)
 106         LOCAL_WORD(saved_spsr_abt)
 107
 108         LOCAL_WORD(saved_context_r13_und)
 109         LOCAL_WORD(saved_context_r14_und)
 110         LOCAL_WORD(saved_spsr_und)
 111 //      LOCAL_WORD(nr_copy_pages)
 112 //      LOCAL_WORD(pagedir_nosave)
 113
 114 /*
 115  * non volatile fpu registers
 116  * s16 - s31
 117  */
 118         /* XXX:TBD */
 119
 120 FUNC(swsusp_arch_suspend)
 121
 122         /* save current program status register */
 123         ldr     r3, .Lsaved_cpsr
 124         mrs     r1, cpsr
 125         str     r1, [r3]
 126
 127         /* hold current mode */
 128         mrs     r0, cpsr
 129
 130         CHANGE_MODE(1f) /* change to system(user) mode */
 131         /* save nonvolatile int register */
 132         ldr     r3, .Lsaved_context_r0
 133         stmia   r3, {r0-r14}
 134
 135         CHANGE_MODE(11) /* change to fiq mode */
 136         /* save nonvolatile int register */
 137         ldr     r3, .Lsaved_context_r8_fiq
 138         stmia   r3, {r8-r14}
 139         /* save spsr_fiq register */
 140         ldr     r3, .Lsaved_spsr_fiq
 141         mrs     r1, spsr
 142         str     r1, [r3]
 143
 144         CHANGE_MODE(12) /* change to irq mode */
 145         /* save nonvolatile int register */
 146         ldr     r3, .Lsaved_context_r13_irq
 147         stmia   r3, {r13-r14}
 148         /* save spsr_irq register */
 149         ldr     r3, .Lsaved_spsr_irq
 150         mrs     r1, spsr
 151         str     r1, [r3]
 152
 153         CHANGE_MODE(13) /* change to svc mode */
 154         /* save nonvolatile int register */
 155         ldr     r3, .Lsaved_context_r13_svc
 156         stmia   r3, {r13-r14}
 157         /* save spsr_svc register */
 158         ldr     r3, .Lsaved_spsr_svc
 159         mrs     r1, spsr
 160         str     r1, [r3]
 161
 162         CHANGE_MODE(17) /* change to abt mode */
 163         /* save nonvolatile int register */
 164         ldr     r3, .Lsaved_context_r13_abt
 165         stmia   r3, {r13-r14}
 166         /* save spsr_abt register */
 167         ldr     r3, .Lsaved_spsr_abt
 168         mrs     r1, spsr
 169         str     r1, [r3]
 170
 171         CHANGE_MODE(1b) /* change to und mode */
 172         /* save nonvolatile int register */
 173         ldr     r3, .Lsaved_context_r13_und
 174         stmia   r3, {r13-r14}
 175         /* save spsr_und register */
 176         ldr     r3, .Lsaved_spsr_und
 177         mrs     r1, spsr
 178         str     r1, [r3]
 179
 180         /* go back to original mode */
 181         msr     cpsr_c, r0
 182
 183         /*
 184          * save nonvolatile fp registers
 185          * and fp status/system registers, if needed
 186          */
 187         /* XXX:TBD */
 188
 189         /* call swsusp_save */
 190         bl      swsusp_save
 191
 192         /* restore return address */
 193         ldr     r3, .Lsaved_context_r14_svc
 194         ldr     lr, [r3]
 195         mov     pc, lr
 196
 197         WORD_ADDR(saved_context_r0)
 198         WORD_ADDR(saved_cpsr)
 199         WORD_ADDR(saved_context_r8_fiq)
 200         WORD_ADDR(saved_spsr_fiq)
 201         WORD_ADDR(saved_context_r13_irq)
 202         WORD_ADDR(saved_spsr_irq)
 203         WORD_ADDR(saved_context_r13_svc)
 204         WORD_ADDR(saved_context_r14_svc)
 205         WORD_ADDR(saved_spsr_svc)
 206         WORD_ADDR(saved_context_r13_abt)
 207         WORD_ADDR(saved_spsr_abt)
 208         WORD_ADDR(saved_context_r13_und)
 209         WORD_ADDR(saved_spsr_und)
 210
 211 FUNC_END(swsusp_arch_suspend)
 212
 213 #define KERNEL_RAM_PADDR        (PHYS_OFFSET + TEXT_OFFSET)
 214 #ifdef CONFIG_ARM_LPAE
 215         /* LPAE requires an additional page for the PGD */
 216 #define PG_DIR_SIZE     0x5000
 217 #define PMD_ORDER       3
 218 #else
 219 #define PG_DIR_SIZE     0x4000
 220 #define PMD_ORDER       2
 221 #endif
 222
 223 #define SWAPPER_PG_DIR          (KERNEL_RAM_PADDR - PG_DIR_SIZE)
 224
 225 FUNC(swsusp_arch_resume)
 226         /* set page table if needed */
 227         ldr     r0, =SWAPPER_PG_DIR
 228         mcr     p15, 0, r0, c2, c0, 0           @ load page table pointer
 229         mcr     p15, 0, r0, c8, c7, 0           @ invalidate I,D TLBs
 230         mcr     p15, 0, r0, c7, c5, 4           @ ISB
 231         orr     r0, r0, #0x59
 232         mcr     p15, 0, r0, c2, c0, 0           @ load page table pointer
 233
 234         /*
 235          * retore "nr_copy_pages" pages which are saved and specified
 236          * at "pagedir_nosave"
 237          */
 238
 239         ldr     r0, .Lrestore_pblist
 240         ldr     r6, [r0]
 241         cmp     r6, #0
 242         beq     .Lnothing_to_copy
 243
 244 .Lcopy_loop:
 245         ldr     r4, [r6, #PBE_ADDRESS]     /* src */
 246         ldr     r5, [r6, #PBE_ORIG_ADDRESS] /* dst */
 247         mov     r9, #1024
 248
 249         /* this loop could be optimized by using stm and ldm. */
 250 .Lcopy_one_page:
 251         ldr     r8, [r4]
 252         str     r8, [r5]
 253
 254         add     r4, r4, #4
 255         add     r5, r5, #4
 256         sub     r9, r9, #1
 257         cmp     r9, #0
 258         bne     .Lcopy_one_page
 259
 260         ldr     r6, [r6, #PBE_NEXT] /* 16 means sizeof(suspend_pagedir_t) */
 261         cmp     r6, #0
 262         bne     .Lcopy_loop
 263
 264 .Lnothing_to_copy:
 265         /* hold current mode */
 266         mrs     r0, cpsr
 267
 268         CHANGE_MODE(1f) /* change to system(user) mode */
 269         /* restore nonvolatile int register */
 270         ldr     r3, .Lsaved_context_r0
 271         ldmia   r3, {r0-r14}
 272         /* restore current program status register */
 273         ldr     r3, .Lsaved_cpsr
 274         ldr     r1, [r3]
 275         msr     cpsr_cxsf, r1
 276
 277         CHANGE_MODE(11) /* change to fiq mode */
 278         /* restore nonvolatile int register */
 279         ldr     r3, .Lsaved_context_r8_fiq
 280         ldmia   r3, {r8-r14}
 281         /* restore spsr_fiq register */
 282         ldr     r3, .Lsaved_spsr_fiq
 283         ldr     r1, [r3]
 284         msr     spsr_cxsf, r1
 285
 286         CHANGE_MODE(12) /* change to irq mode */
 287         /* restore nonvolatile int register */
 288         ldr     r3, .Lsaved_context_r13_irq
 289         ldmia   r3, {r13-r14}
 290         /* restore spsr_irq register */
 291         ldr     r3, .Lsaved_spsr_irq
 292         ldr     r1, [r3]
 293         msr     spsr_cxsf, r1
 294
 295         CHANGE_MODE(13) /* change to svc mode */
 296         /* restore nonvolatile int register */
 297         ldr     r3, .Lsaved_context_r13_svc
 298         ldmia   r3, {r13-r14}
 299         /* restore spsr_svc register */
 300         ldr     r3, .Lsaved_spsr_svc
 301         ldr     r1, [r3]
 302         msr     spsr_cxsf, r1
 303
 304         CHANGE_MODE(17) /* change to abt mode */
 305         /* restore nonvolatile int register */
 306         ldr     r3, .Lsaved_context_r13_abt
 307         ldmia   r3, {r13-r14}
 308         /* restore spsr_abt register */
 309         ldr     r3, .Lsaved_spsr_abt
 310         ldr     r1, [r3]
 311         msr     spsr_cxsf, r1
 312
 313         CHANGE_MODE(1b) /* change to und mode */
 314         /* restore nonvolatile int register */
 315         ldr     r3, .Lsaved_context_r13_und
 316         ldmia   r3, {r13-r14}
 317         /* restore spsr_und register */
 318         ldr     r3, .Lsaved_spsr_und
 319         ldr     r1, [r3]
 320         msr     spsr_cxsf, r1
 321
 322         /* go back to original mode */
 323         msr     cpsr_c, r0
 324
 325         /*
 326          * restore nonvolatile fp registers
 327          *  and fp status/system registers, if needed
 328          */
 329         /* XXX:TBD */
 330
 331         /* call swsusp_restore */
 332 //      bl      swsusp_restore
 333
 334         /* r0 will be the return value of swsusp_arch_suspend */
 335         mov     r0, #0
 336         /* restore return address */
 337         ldr     r3, .Lsaved_context_r14_svc
 338         ldr     lr, [r3]
 339         mov     pc, lr
 340
 341         WORD_ADDR(restore_pblist)
 342
 343 FUNC_END(swsusp_arch_resume)