arch/arm/common/fiq_glue.S

   1 /*
   2  * Copyright (C) 2008 Google, Inc.
   3  *
   4  * This software is licensed under the terms of the GNU General Public
   5  * License version 2, as published by the Free Software Foundation, and
   6  * may be copied, distributed, and modified under those terms.
   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  */
  14
  15 #include <linux/linkage.h>
  16 #include <asm/assembler.h>
  17
  18                 .text
  19
  20                 .global fiq_glue_end
  21
  22                 /* fiq stack: r0-r15,cpsr,spsr of interrupted mode */
  23
  24 ENTRY(fiq_glue)
  25                 /* store pc, cpsr from previous mode */
  26                 mrs     r12, spsr
  27                 sub     r11, lr, #4
  28                 subs    r10, #1
  29                 bne     nested_fiq
  30
  31                 stmfd   sp!, {r11-r12, lr}
  32
  33                 /* store r8-r14 from previous mode */
  34                 sub     sp, sp, #(7 * 4)
  35                 stmia   sp, {r8-r14}^
  36                 nop
  37
  38                 /* store r0-r7 from previous mode */
  39                 stmfd   sp!, {r0-r7}
  40
  41                 /* setup func(data,regs) arguments */
  42                 mov     r0, r9
  43                 mov     r1, sp
  44                 mov     r3, r8
  45
  46                 mov     r7, sp
  47
  48                 /* Get sp and lr from non-user modes */
  49                 and     r4, r12, #MODE_MASK
  50                 cmp     r4, #USR_MODE
  51                 beq     fiq_from_usr_mode
  52
  53                 mov     r7, sp
  54                 orr     r4, r4, #(PSR_I_BIT | PSR_F_BIT)
  55                 msr     cpsr_c, r4
  56                 str     sp, [r7, #(4 * 13)]
  57                 str     lr, [r7, #(4 * 14)]
  58                 mrs     r5, spsr
  59                 str     r5, [r7, #(4 * 17)]
  60
  61                 cmp     r4, #(SVC_MODE | PSR_I_BIT | PSR_F_BIT)
  62                 /* use fiq stack if we reenter this mode */
  63                 subne   sp, r7, #(4 * 3)
  64
  65 fiq_from_usr_mode:
  66                 msr     cpsr_c, #(SVC_MODE | PSR_I_BIT | PSR_F_BIT)
  67                 mov     r2, sp
  68                 sub     sp, r7, #12
  69                 stmfd   sp!, {r2, ip, lr}
  70                 /* call func(data,regs) */
  71                 blx     r3
  72                 ldmfd   sp, {r2, ip, lr}
  73                 mov     sp, r2
  74
  75                 /* restore/discard saved state */
  76                 cmp     r4, #USR_MODE
  77                 beq     fiq_from_usr_mode_exit
  78
  79                 msr     cpsr_c, r4
  80                 ldr     sp, [r7, #(4 * 13)]
  81                 ldr     lr, [r7, #(4 * 14)]
  82                 msr     spsr_cxsf, r5
  83
  84 fiq_from_usr_mode_exit:
  85                 msr     cpsr_c, #(FIQ_MODE | PSR_I_BIT | PSR_F_BIT)
  86
  87                 ldmfd   sp!, {r0-r7}
  88                 add     sp, sp, #(7 * 4)
  89                 ldmfd   sp!, {r11-r12, lr}
  90 exit_fiq:
  91                 msr     spsr_cxsf, r12
  92                 add     r10, #1
  93                 movs    pc, r11
  94
  95 nested_fiq:
  96                 orr     r12, r12, #(PSR_F_BIT)
  97                 b       exit_fiq
  98
  99 fiq_glue_end:
 100
 101 ENTRY(fiq_glue_setup) /* func, data, sp */
 102                 mrs             r3, cpsr
 103                 msr             cpsr_c, #(FIQ_MODE | PSR_I_BIT | PSR_F_BIT)
 104                 movs            r8, r0
 105                 mov             r9, r1
 106                 mov             sp, r2
 107                 moveq           r10, #0
 108                 movne           r10, #1
 109                 msr             cpsr_c, r3
 110                 bx              lr
 111