[PATCH] ppc32: add 405EP cpu_spec entry
[linux-2.6] / arch / ppc / kernel / vector.S
1 #include <asm/ppc_asm.h>
2 #include <asm/processor.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         .data
10 fpzero:
11         .long   0
12 fpone:
13         .long   0x3f800000      /* 1.0 in single-precision FP */
14 fphalf:
15         .long   0x3f000000      /* 0.5 in single-precision FP */
16
17         .text
18 /*
19  * Internal routine to enable floating point and set FPSCR to 0.
20  * Don't call it from C; it doesn't use the normal calling convention.
21  */
22 fpenable:
23         mfmsr   r10
24         ori     r11,r10,MSR_FP
25         mtmsr   r11
26         isync
27         stfd    fr0,24(r1)
28         stfd    fr1,16(r1)
29         stfd    fr31,8(r1)
30         lis     r11,fpzero@ha
31         mffs    fr31
32         lfs     fr1,fpzero@l(r11)
33         mtfsf   0xff,fr1
34         blr
35
36 fpdisable:
37         mtfsf   0xff,fr31
38         lfd     fr31,8(r1)
39         lfd     fr1,16(r1)
40         lfd     fr0,24(r1)
41         mtmsr   r10
42         isync
43         blr
44
45 /*
46  * Vector add, floating point.
47  */
48         .globl  vaddfp
49 vaddfp:
50         stwu    r1,-32(r1)
51         mflr    r0
52         stw     r0,36(r1)
53         bl      fpenable
54         li      r0,4
55         mtctr   r0
56         li      r6,0
57 1:      lfsx    fr0,r4,r6
58         lfsx    fr1,r5,r6
59         fadds   fr0,fr0,fr1
60         stfsx   fr0,r3,r6
61         addi    r6,r6,4
62         bdnz    1b
63         bl      fpdisable
64         lwz     r0,36(r1)
65         mtlr    r0
66         addi    r1,r1,32
67         blr
68
69 /*
70  * Vector subtract, floating point.
71  */
72         .globl  vsubfp
73 vsubfp:
74         stwu    r1,-32(r1)
75         mflr    r0
76         stw     r0,36(r1)
77         bl      fpenable
78         li      r0,4
79         mtctr   r0
80         li      r6,0
81 1:      lfsx    fr0,r4,r6
82         lfsx    fr1,r5,r6
83         fsubs   fr0,fr0,fr1
84         stfsx   fr0,r3,r6
85         addi    r6,r6,4
86         bdnz    1b
87         bl      fpdisable
88         lwz     r0,36(r1)
89         mtlr    r0
90         addi    r1,r1,32
91         blr
92
93 /*
94  * Vector multiply and add, floating point.
95  */
96         .globl  vmaddfp
97 vmaddfp:
98         stwu    r1,-48(r1)
99         mflr    r0
100         stw     r0,52(r1)
101         bl      fpenable
102         stfd    fr2,32(r1)
103         li      r0,4
104         mtctr   r0
105         li      r7,0
106 1:      lfsx    fr0,r4,r7
107         lfsx    fr1,r5,r7
108         lfsx    fr2,r6,r7
109         fmadds  fr0,fr0,fr2,fr1
110         stfsx   fr0,r3,r7
111         addi    r7,r7,4
112         bdnz    1b
113         lfd     fr2,32(r1)
114         bl      fpdisable
115         lwz     r0,52(r1)
116         mtlr    r0
117         addi    r1,r1,48
118         blr
119
120 /*
121  * Vector negative multiply and subtract, floating point.
122  */
123         .globl  vnmsubfp
124 vnmsubfp:
125         stwu    r1,-48(r1)
126         mflr    r0
127         stw     r0,52(r1)
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         bl      fpdisable
142         lwz     r0,52(r1)
143         mtlr    r0
144         addi    r1,r1,48
145         blr
146
147 /*
148  * Vector reciprocal estimate.  We just compute 1.0/x.
149  * r3 -> destination, r4 -> source.
150  */
151         .globl  vrefp
152 vrefp:
153         stwu    r1,-32(r1)
154         mflr    r0
155         stw     r0,36(r1)
156         bl      fpenable
157         lis     r9,fpone@ha
158         li      r0,4
159         lfs     fr1,fpone@l(r9)
160         mtctr   r0
161         li      r6,0
162 1:      lfsx    fr0,r4,r6
163         fdivs   fr0,fr1,fr0
164         stfsx   fr0,r3,r6
165         addi    r6,r6,4
166         bdnz    1b
167         bl      fpdisable
168         lwz     r0,36(r1)
169         mtlr    r0
170         addi    r1,r1,32
171         blr
172
173 /*
174  * Vector reciprocal square-root estimate, floating point.
175  * We use the frsqrte instruction for the initial estimate followed
176  * by 2 iterations of Newton-Raphson to get sufficient accuracy.
177  * r3 -> destination, r4 -> source.
178  */
179         .globl  vrsqrtefp
180 vrsqrtefp:
181         stwu    r1,-48(r1)
182         mflr    r0
183         stw     r0,52(r1)
184         bl      fpenable
185         stfd    fr2,32(r1)
186         stfd    fr3,40(r1)
187         stfd    fr4,48(r1)
188         stfd    fr5,56(r1)
189         lis     r9,fpone@ha
190         lis     r8,fphalf@ha
191         li      r0,4
192         lfs     fr4,fpone@l(r9)
193         lfs     fr5,fphalf@l(r8)
194         mtctr   r0
195         li      r6,0
196 1:      lfsx    fr0,r4,r6
197         frsqrte fr1,fr0         /* r = frsqrte(s) */
198         fmuls   fr3,fr1,fr0     /* r * s */
199         fmuls   fr2,fr1,fr5     /* r * 0.5 */
200         fnmsubs fr3,fr1,fr3,fr4 /* 1 - s * r * r */
201         fmadds  fr1,fr2,fr3,fr1 /* r = r + 0.5 * r * (1 - s * r * r) */
202         fmuls   fr3,fr1,fr0     /* r * s */
203         fmuls   fr2,fr1,fr5     /* r * 0.5 */
204         fnmsubs fr3,fr1,fr3,fr4 /* 1 - s * r * r */
205         fmadds  fr1,fr2,fr3,fr1 /* r = r + 0.5 * r * (1 - s * r * r) */
206         stfsx   fr1,r3,r6
207         addi    r6,r6,4
208         bdnz    1b
209         lfd     fr5,56(r1)
210         lfd     fr4,48(r1)
211         lfd     fr3,40(r1)
212         lfd     fr2,32(r1)
213         bl      fpdisable
214         lwz     r0,36(r1)
215         mtlr    r0
216         addi    r1,r1,32
217         blr