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, reserve space for spsr */
  26                 mrs     r12, spsr
  27                 sub     lr, lr, #4
  28                 subs    r10, #1
  29                 bne     nested_fiq
  30
  31                 str     r12, [sp, #-8]!
  32                 str     lr, [sp, #-4]!
  33
  34                 /* store r8-r14 from previous mode */
  35                 sub     sp, sp, #(7 * 4)
  36                 stmia   sp, {r8-r14}^
  37                 nop
  38
  39                 /* store r0-r7 from previous mode */
  40                 stmfd   sp!, {r0-r7}
  41
  42                 /* setup func(data,regs) arguments */
  43                 mov     r0, r9
  44                 mov     r1, sp
  45                 mov     r3, r8
  46
  47                 mov     r7, sp
  48
  49                 /* Get sp and lr from non-user modes */
  50                 and     r4, r12, #MODE_MASK
  51                 cmp     r4, #USR_MODE
  52                 beq     fiq_from_usr_mode
  53
  54                 mov     r7, sp
  55                 orr     r4, r4, #(PSR_I_BIT | PSR_F_BIT)
  56                 msr     cpsr_c, r4
  57                 str     sp, [r7, #(4 * 13)]
  58                 str     lr, [r7, #(4 * 14)]
  59                 mrs     r5, spsr
  60                 str     r5, [r7, #(4 * 17)]
  61
  62                 cmp     r4, #(SVC_MODE | PSR_I_BIT | PSR_F_BIT)
  63                 /* use fiq stack if we reenter this mode */
  64                 subne   sp, r7, #(4 * 3)
  65
  66 fiq_from_usr_mode:
  67                 msr     cpsr_c, #(SVC_MODE | PSR_I_BIT | PSR_F_BIT)
  68                 mov     r2, sp
  69                 sub     sp, r7, #12
  70                 stmfd   sp!, {r2, ip, lr}
  71                 /* call func(data,regs) */
  72                 blx     r3
  73                 ldmfd   sp, {r2, ip, lr}
  74                 mov     sp, r2
  75
  76                 /* restore/discard saved state */
  77                 cmp     r4, #USR_MODE
  78                 beq     fiq_from_usr_mode_exit
  79
  80                 msr     cpsr_c, r4
  81                 ldr     sp, [r7, #(4 * 13)]
  82                 ldr     lr, [r7, #(4 * 14)]
  83                 msr     spsr_cxsf, r5
  84
  85 fiq_from_usr_mode_exit:
  86                 msr     cpsr_c, #(FIQ_MODE | PSR_I_BIT | PSR_F_BIT)
  87
  88                 ldmfd   sp!, {r0-r7}
  89                 ldr     lr, [sp, #(4 * 7)]
  90                 ldr     r12, [sp, #(4 * 8)]
  91                 add     sp, sp, #(10 * 4)
  92 exit_fiq:
  93                 msr     spsr_cxsf, r12
  94                 add     r10, #1
  95                 cmp     r11, #0
  96                 moveqs  pc, lr
  97                 bx      r11 /* jump to custom fiq return function */
  98
  99 nested_fiq:
 100                 orr     r12, r12, #(PSR_F_BIT)
 101                 b       exit_fiq
 102
 103 fiq_glue_end:
 104
 105 ENTRY(fiq_glue_setup) /* func, data, sp, smc call number */
 106                 stmfd           sp!, {r4}
 107                 mrs             r4, cpsr
 108                 msr             cpsr_c, #(FIQ_MODE | PSR_I_BIT | PSR_F_BIT)
 109                 movs            r8, r0
 110                 mov             r9, r1
 111                 mov             sp, r2
 112                 mov             r11, r3
 113                 moveq           r10, #0
 114                 movne           r10, #1
 115                 msr             cpsr_c, r4
 116                 ldmfd           sp!, {r4}
 117                 bx              lr
 118