summaryrefslogtreecommitdiff
path: root/arch/ppc/kernel/vector.S
blob: 82a21346bf8052ee33163b4c6277c2a8070131c4 (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
#include <asm/ppc_asm.h>
#include <asm/processor.h>

/*
 * The routines below are in assembler so we can closely control the
 * usage of floating-point registers.  These routines must be called
 * with preempt disabled.
 */
	.data
fpzero:
	.long	0
fpone:
	.long	0x3f800000	/* 1.0 in single-precision FP */
fphalf:
	.long	0x3f000000	/* 0.5 in single-precision FP */

	.text
/*
 * Internal routine to enable floating point and set FPSCR to 0.
 * Don't call it from C; it doesn't use the normal calling convention.
 */
fpenable:
	mfmsr	r10
	ori	r11,r10,MSR_FP
	mtmsr	r11
	isync
	stfd	fr0,24(r1)
	stfd	fr1,16(r1)
	stfd	fr31,8(r1)
	lis	r11,fpzero@ha
	mffs	fr31
	lfs	fr1,fpzero@l(r11)
	mtfsf	0xff,fr1
	blr

fpdisable:
	mtfsf	0xff,fr31
	lfd	fr31,8(r1)
	lfd	fr1,16(r1)
	lfd	fr0,24(r1)
	mtmsr	r10
	isync
	blr

/*
 * Vector add, floating point.
 */
	.globl	vaddfp
vaddfp:
	stwu	r1,-32(r1)
	mflr	r0
	stw	r0,36(r1)
	bl	fpenable
	li	r0,4
	mtctr	r0
	li	r6,0
1:	lfsx	fr0,r4,r6
	lfsx	fr1,r5,r6
	fadds	fr0,fr0,fr1
	stfsx	fr0,r3,r6
	addi	r6,r6,4
	bdnz	1b
	bl	fpdisable
	lwz	r0,36(r1)
	mtlr	r0
	addi	r1,r1,32
	blr

/*
 * Vector subtract, floating point.
 */
	.globl	vsubfp
vsubfp:
	stwu	r1,-32(r1)
	mflr	r0
	stw	r0,36(r1)
	bl	fpenable
	li	r0,4
	mtctr	r0
	li	r6,0
1:	lfsx	fr0,r4,r6
	lfsx	fr1,r5,r6
	fsubs	fr0,fr0,fr1
	stfsx	fr0,r3,r6
	addi	r6,r6,4
	bdnz	1b
	bl	fpdisable
	lwz	r0,36(r1)
	mtlr	r0
	addi	r1,r1,32
	blr

/*
 * Vector multiply and add, floating point.
 */
	.globl	vmaddfp
vmaddfp:
	stwu	r1,-48(r1)
	mflr	r0
	stw	r0,52(r1)
	bl	fpenable
	stfd	fr2,32(r1)
	li	r0,4
	mtctr	r0
	li	r7,0
1:	lfsx	fr0,r4,r7
	lfsx	fr1,r5,r7
	lfsx	fr2,r6,r7
	fmadds	fr0,fr0,fr2,fr1
	stfsx	fr0,r3,r7
	addi	r7,r7,4
	bdnz	1b
	lfd	fr2,32(r1)
	bl	fpdisable
	lwz	r0,52(r1)
	mtlr	r0
	addi	r1,r1,48
	blr

/*
 * Vector negative multiply and subtract, floating point.
 */
	.globl	vnmsubfp
vnmsubfp:
	stwu	r1,-48(r1)
	mflr	r0
	stw	r0,52(r1)
	bl	fpenable
	stfd	fr2,32(r1)
	li	r0,4
	mtctr	r0
	li	r7,0
1:	lfsx	fr0,r4,r7
	lfsx	fr1,r5,r7
	lfsx	fr2,r6,r7
	fnmsubs	fr0,fr0,fr2,fr1
	stfsx	fr0,r3,r7
	addi	r7,r7,4
	bdnz	1b
	lfd	fr2,32(r1)
	bl	fpdisable
	lwz	r0,52(r1)
	mtlr	r0
	addi	r1,r1,48
	blr

/*
 * Vector reciprocal estimate.  We just compute 1.0/x.
 * r3 -> destination, r4 -> source.
 */
	.globl	vrefp
vrefp:
	stwu	r1,-32(r1)
	mflr	r0
	stw	r0,36(r1)
	bl	fpenable
	lis	r9,fpone@ha
	li	r0,4
	lfs	fr1,fpone@l(r9)
	mtctr	r0
	li	r6,0
1:	lfsx	fr0,r4,r6
	fdivs	fr0,fr1,fr0
	stfsx	fr0,r3,r6
	addi	r6,r6,4
	bdnz	1b
	bl	fpdisable
	lwz	r0,36(r1)
	mtlr	r0
	addi	r1,r1,32
	blr

/*
 * Vector reciprocal square-root estimate, floating point.
 * We use the frsqrte instruction for the initial estimate followed
 * by 2 iterations of Newton-Raphson to get sufficient accuracy.
 * r3 -> destination, r4 -> source.
 */
	.globl	vrsqrtefp
vrsqrtefp:
	stwu	r1,-48(r1)
	mflr	r0
	stw	r0,52(r1)
	bl	fpenable
	stfd	fr2,32(r1)
	stfd	fr3,40(r1)
	stfd	fr4,48(r1)
	stfd	fr5,56(r1)
	lis	r9,fpone@ha
	lis	r8,fphalf@ha
	li	r0,4
	lfs	fr4,fpone@l(r9)
	lfs	fr5,fphalf@l(r8)
	mtctr	r0
	li	r6,0
1:	lfsx	fr0,r4,r6
	frsqrte	fr1,fr0		/* r = frsqrte(s) */
	fmuls	fr3,fr1,fr0	/* r * s */
	fmuls	fr2,fr1,fr5	/* r * 0.5 */
	fnmsubs	fr3,fr1,fr3,fr4	/* 1 - s * r * r */
	fmadds	fr1,fr2,fr3,fr1	/* r = r + 0.5 * r * (1 - s * r * r) */
	fmuls	fr3,fr1,fr0	/* r * s */
	fmuls	fr2,fr1,fr5	/* r * 0.5 */
	fnmsubs	fr3,fr1,fr3,fr4	/* 1 - s * r * r */
	fmadds	fr1,fr2,fr3,fr1	/* r = r + 0.5 * r * (1 - s * r * r) */
	stfsx	fr1,r3,r6
	addi	r6,r6,4
	bdnz	1b
	lfd	fr5,56(r1)
	lfd	fr4,48(r1)
	lfd	fr3,40(r1)
	lfd	fr2,32(r1)
	bl	fpdisable
	lwz	r0,36(r1)
	mtlr	r0
	addi	r1,r1,32
	blr