Merge git://git.infradead.org/mtd-2.6
[linux-2.6] / arch / powerpc / kernel / vector.S
1 #include <asm/ppc_asm.h>
2 #include <asm/reg.h>
3
4 /*
5  * The routines below are in assembler so we can closely control the
6  * usage of floating-point registers.  These routines must be called
7  * with preempt disabled.
8  */
9 #ifdef CONFIG_PPC32
10         .data
11 fpzero:
12         .long   0
13 fpone:
14         .long   0x3f800000      /* 1.0 in single-precision FP */
15 fphalf:
16         .long   0x3f000000      /* 0.5 in single-precision FP */
17
18 #define LDCONST(fr, name)       \
19         lis     r11,name@ha;    \
20         lfs     fr,name@l(r11)
21 #else
22
23         .section ".toc","aw"
24 fpzero:
25         .tc     FD_0_0[TC],0
26 fpone:
27         .tc     FD_3ff00000_0[TC],0x3ff0000000000000    /* 1.0 */
28 fphalf:
29         .tc     FD_3fe00000_0[TC],0x3fe0000000000000    /* 0.5 */
30
31 #define LDCONST(fr, name)       \
32         lfd     fr,name@toc(r2)
33 #endif
34
35         .text
36 /*
37  * Internal routine to enable floating point and set FPSCR to 0.
38  * Don't call it from C; it doesn't use the normal calling convention.
39  */
40 fpenable:
41 #ifdef CONFIG_PPC32
42         stwu    r1,-64(r1)
43 #else
44         stdu    r1,-64(r1)
45 #endif
46         mfmsr   r10
47         ori     r11,r10,MSR_FP
48         mtmsr   r11
49         isync
50         stfd    fr0,24(r1)
51         stfd    fr1,16(r1)
52         stfd    fr31,8(r1)
53         LDCONST(fr1, fpzero)
54         mffs    fr31
55         MTFSF_L(fr1)
56         blr
57
58 fpdisable:
59         mtlr    r12
60         MTFSF_L(fr31)
61         lfd     fr31,8(r1)
62         lfd     fr1,16(r1)
63         lfd     fr0,24(r1)
64         mtmsr   r10
65         isync
66         addi    r1,r1,64
67         blr
68
69 /*
70  * Vector add, floating point.
71  */
72 _GLOBAL(vaddfp)
73         mflr    r12
74         bl      fpenable
75         li      r0,4
76         mtctr   r0
77         li      r6,0
78 1:      lfsx    fr0,r4,r6
79         lfsx    fr1,r5,r6
80         fadds   fr0,fr0,fr1
81         stfsx   fr0,r3,r6
82         addi    r6,r6,4
83         bdnz    1b
84         b       fpdisable
85
86 /*
87  * Vector subtract, floating point.
88  */
89 _GLOBAL(vsubfp)
90         mflr    r12
91         bl      fpenable
92         li      r0,4
93         mtctr   r0
94         li      r6,0
95 1:      lfsx    fr0,r4,r6
96         lfsx    fr1,r5,r6
97         fsubs   fr0,fr0,fr1
98         stfsx   fr0,r3,r6
99         addi    r6,r6,4
100         bdnz    1b
101         b       fpdisable
102
103 /*
104  * Vector multiply and add, floating point.
105  */
106 _GLOBAL(vmaddfp)
107         mflr    r12
108         bl      fpenable
109         stfd    fr2,32(r1)
110         li      r0,4
111         mtctr   r0
112         li      r7,0
113 1:      lfsx    fr0,r4,r7
114         lfsx    fr1,r5,r7
115         lfsx    fr2,r6,r7
116         fmadds  fr0,fr0,fr2,fr1
117         stfsx   fr0,r3,r7
118         addi    r7,r7,4
119         bdnz    1b
120         lfd     fr2,32(r1)
121         b       fpdisable
122
123 /*
124  * Vector negative multiply and subtract, floating point.
125  */
126 _GLOBAL(vnmsubfp)
127         mflr    r12
128         bl      fpenable
129         stfd    fr2,32(r1)
130         li      r0,4
131         mtctr   r0
132         li      r7,0
133 1:      lfsx    fr0,r4,r7
134         lfsx    fr1,r5,r7
135         lfsx    fr2,r6,r7
136         fnmsubs fr0,fr0,fr2,fr1
137         stfsx   fr0,r3,r7
138         addi    r7,r7,4
139         bdnz    1b
140         lfd     fr2,32(r1)
141         b       fpdisable
142
143 /*
144  * Vector reciprocal estimate.  We just compute 1.0/x.
145  * r3 -> destination, r4 -> source.
146  */
147 _GLOBAL(vrefp)
148         mflr    r12
149         bl      fpenable
150         li      r0,4
151         LDCONST(fr1, fpone)
152         mtctr   r0
153         li      r6,0
154 1:      lfsx    fr0,r4,r6
155         fdivs   fr0,fr1,fr0
156         stfsx   fr0,r3,r6
157         addi    r6,r6,4
158         bdnz    1b
159         b       fpdisable
160
161 /*
162  * Vector reciprocal square-root estimate, floating point.
163  * We use the frsqrte instruction for the initial estimate followed
164  * by 2 iterations of Newton-Raphson to get sufficient accuracy.
165  * r3 -> destination, r4 -> source.
166  */
167 _GLOBAL(vrsqrtefp)
168         mflr    r12
169         bl      fpenable
170         stfd    fr2,32(r1)
171         stfd    fr3,40(r1)
172         stfd    fr4,48(r1)
173         stfd    fr5,56(r1)
174         li      r0,4
175         LDCONST(fr4, fpone)
176         LDCONST(fr5, fphalf)
177         mtctr   r0
178         li      r6,0
179 1:      lfsx    fr0,r4,r6
180         frsqrte fr1,fr0         /* r = frsqrte(s) */
181         fmuls   fr3,fr1,fr0     /* r * s */
182         fmuls   fr2,fr1,fr5     /* r * 0.5 */
183         fnmsubs fr3,fr1,fr3,fr4 /* 1 - s * r * r */
184         fmadds  fr1,fr2,fr3,fr1 /* r = r + 0.5 * r * (1 - s * r * r) */
185         fmuls   fr3,fr1,fr0     /* r * s */
186         fmuls   fr2,fr1,fr5     /* r * 0.5 */
187         fnmsubs fr3,fr1,fr3,fr4 /* 1 - s * r * r */
188         fmadds  fr1,fr2,fr3,fr1 /* r = r + 0.5 * r * (1 - s * r * r) */
189         stfsx   fr1,r3,r6
190         addi    r6,r6,4
191         bdnz    1b
192         lfd     fr5,56(r1)
193         lfd     fr4,48(r1)
194         lfd     fr3,40(r1)
195         lfd     fr2,32(r1)
196         b       fpdisable