Way faster FDIV/FSQRT on SPARC
[akaros.git] / kern / arch / sparc / recip.S
1 // Very convoluted FP sqrt/div emulation code.
2 // Newton-Raphson method.
3
4 #include <arch/trap_table.h>
5 #include <arch/arch.h>
6 #include <ros/memlayout.h>
7
8 #define SET_RDD(src)             \
9         srl     %l0,25,%l3      ;\
10         and     %l3,0x1E,%l3    ;\
11         sll     %l3,2,%l3       ;\
12         add     %l6,%l3,%l3     ;\
13         std     src,[%l3+0x80]
14
15 #define GET_RS1D(dest)           \
16         srl     %l0,14,%l3      ;\
17         and     %l3,0x1E,%l3    ;\
18         sll     %l3,2,%l3       ;\
19         add     %l6,%l3,%l3     ;\
20         WEIRDD_ADDR(%l3+0x80)   ;\
21         ldd     [%l3+0x80],dest
22
23 #define GET_RS2D(dest)           \
24         and     %l0,0x1E,%l3    ;\
25         sll     %l3,2,%l3       ;\
26         add     %l6,%l3,%l3     ;\
27         WEIRDD_ADDR(%l3+0x80)   ;\
28         ldd     [%l3+0x80],dest
29
30 #define SET_RD(src)              \
31         srl     %l0,25,%l3      ;\
32         and     %l3,0x1F,%l3    ;\
33         sll     %l3,2,%l3       ;\
34         add     %l6,%l3,%l3     ;\
35         st      src,[%l3+0x80]
36
37 #define GET_RS1(dest)            \
38         srl     %l0,14,%l3      ;\
39         and     %l3,0x1F,%l3    ;\
40         sll     %l3,2,%l3       ;\
41         add     %l6,%l3,%l3     ;\
42         WEIRD_ADDR(%l3+0x80)    ;\
43         ld      [%l3+0x80],dest
44
45 #define GET_RS2(dest)            \
46         and     %l0,0x1F,%l3    ;\
47         sll     %l3,2,%l3       ;\
48         add     %l6,%l3,%l3     ;\
49         WEIRD_ADDR(%l3+0x80)    ;\
50         ld      [%l3+0x80],dest
51
52 #define RESTORE_STATE            \
53         ld      [%l6+64],%l1    ;\
54         ld      [%l6+68],%l2    ;\
55         ld      [%l6+72],%l3    ;\
56         ld      [%l6+76],%fsr   ;\
57         mov     %l3,%psr        ;\
58
59 #define SAVE_FP_REGS             \
60         std     %f0, [%l6+0x80] ;\
61         std     %f2, [%l6+0x88] ;\
62         std     %f4, [%l6+0x90] ;\
63         std     %f6, [%l6+0x98] ;\
64         std     %f8, [%l6+0xA0] ;\
65         std     %f10,[%l6+0xA8] ;\
66         std     %f12,[%l6+0xB0] ;\
67         std     %f14,[%l6+0xB8] ;\
68         std     %f16,[%l6+0xC0] ;\
69         std     %f18,[%l6+0xC8] ;\
70         std     %f20,[%l6+0xD0] ;\
71         std     %f22,[%l6+0xD8] ;\
72         std     %f24,[%l6+0xE0] ;\
73         std     %f26,[%l6+0xE8] ;\
74         std     %f28,[%l6+0xF0] ;\
75         std     %f30,[%l6+0xF8]
76
77 #define RESTORE_FP_REGS          \
78         ldd     [%l6+0x80],%f0  ;\
79         ldd     [%l6+0x88],%f2  ;\
80         ldd     [%l6+0x90],%f4  ;\
81         ldd     [%l6+0x98],%f6  ;\
82         ldd     [%l6+0xA0],%f8  ;\
83         ldd     [%l6+0xA8],%f10 ;\
84         ldd     [%l6+0xB0],%f12 ;\
85         ldd     [%l6+0xB8],%f14 ;\
86         ldd     [%l6+0xC0],%f16 ;\
87         ldd     [%l6+0xC8],%f18 ;\
88         ldd     [%l6+0xD0],%f20 ;\
89         ldd     [%l6+0xD8],%f22 ;\
90         ldd     [%l6+0xE0],%f24 ;\
91         ldd     [%l6+0xE8],%f26 ;\
92         ldd     [%l6+0xF0],%f28 ;\
93         ldd     [%l6+0xF8],%f30
94
95 // under odd circumstances, emulate it
96 #define WEIRDD(reg)              \
97         std     reg,[%l6+112]   ;\
98         WEIRDD_ADDR(%l6+112)
99
100 #define WEIRDD_ADDR(addr)        \
101         ld      [addr],%l1      ;\
102         sll     %l1,1,%l1       ;\
103         srl     %l1,21,%l1      ;\
104         cmp     %l1,0x7FF       ;\
105         be      giveup          ;\
106         tst     %l1             ;\
107         be      giveup
108
109 #define WEIRD_ADDR(addr)         \
110         ld      [addr],%l1      ;\
111         sll     %l1,1,%l1       ;\
112         srl     %l1,24,%l1      ;\
113         cmp     %l1,0xFF        ;\
114         be      giveup          ;\
115         tst     %l1             ;\
116         be      giveup
117
118 .global fast_fp_exception
119 fast_fp_exception:
120
121         // get the instruction (no fault mode)
122         lda     [%g0] 4, %l3
123         or      %l3,2,%l0
124         sta     %l0, [%g0] 4
125         ld      [%l1], %l0
126         sta     %l3, [%g0] 4
127
128         set     bootstacktop-256,%l6
129         mov     CORE_ID_REG,%l7
130         sll     %l7,KSTKSHIFT,%l7
131         sub     %l6,%l7,%l6
132
133         mov     %psr,%l3
134         st      %l1,[%l6+64]
135         st      %l2,[%l6+68]
136         st      %l3,[%l6+72]
137         st      %fsr,[%l6+76]
138
139         // decode the instruction
140         set     0x81F83FE0, %l3         ! opcode mask
141         and     %l3,%l0,%l3             !
142         set     0x81A009A0, %l4         ! fdivs?
143         set     0x81A009C0, %l5         ! fdivd?
144         set     0x81A00520, %l1         ! fsqrts?
145         set     0x81A00540, %l7         ! fsqrtd?
146         cmp     %l3,%l4
147         be      do_fdivs
148         cmp     %l3,%l5
149         be      do_fdivd
150         cmp     %l3,%l1
151         be      do_fsqrts
152         cmp     %l3,%l7
153         be      do_fsqrtd
154
155         b,a     getout
156         // nothing we can handle fast; call fp_exception
157 giveup:
158         RESTORE_FP_REGS
159 getout:
160         RESTORE_STATE
161         TRAP_TABLE_ENTRY(fp_exception)
162
163 do_fdivs:
164         SAVE_FP_REGS
165         st      %g0,[%l6+124]
166         ld      [%l6+124],%fsr
167         GET_RS2(%f0)
168         fstod   %f0,%f0
169         sethi   %hi(recip_asm),%l1
170         jmpl    %l1+%lo(recip_asm),%l7
171          nop
172         GET_RS1(%f2)
173         fstod   %f2,%f2
174         fmuld   %f2,%f0,%f0
175         WEIRDD(%f0)
176         fdtos   %f0,%f0
177         SET_RD(%f0)
178         RESTORE_FP_REGS
179         RESTORE_STATE
180         jmp     %l2
181          rett   %l2+4
182
183 do_fdivd:
184         SAVE_FP_REGS
185         st      %g0,[%l6+124]
186         ld      [%l6+124],%fsr
187         GET_RS2D(%f0)
188         sethi   %hi(recip_asm),%l1
189         jmpl    %l1+%lo(recip_asm),%l7
190          nop
191         GET_RS1D(%f2)
192         fmuld   %f2,%f0,%f0
193         WEIRDD(%f0)
194         SET_RDD(%f0)
195         RESTORE_FP_REGS
196         RESTORE_STATE
197         jmp     %l2
198          rett   %l2+4
199
200 do_fsqrts:
201         SAVE_FP_REGS
202         st      %g0,[%l6+124]
203         ld      [%l6+124],%fsr
204         GET_RS2(%f0)
205         fstod   %f0,%f0
206         sethi   %hi(recip_sqrt_asm),%l1
207         jmpl    %l1+%lo(recip_sqrt_asm),%l7
208          nop
209         GET_RS2(%f2)
210         fstod   %f2,%f2
211         fmuld   %f2,%f0,%f0
212         WEIRDD(%f0)
213         fdtos   %f0,%f0
214         SET_RD(%f0)
215         RESTORE_FP_REGS
216         RESTORE_STATE
217         jmp     %l2
218          rett   %l2+4
219
220 do_fsqrtd:
221         SAVE_FP_REGS
222         st      %g0,[%l6+124]
223         ld      [%l6+124],%fsr
224         GET_RS2D(%f0)
225         sethi   %hi(recip_sqrt_asm),%l1
226         jmpl    %l1+%lo(recip_sqrt_asm),%l7
227          nop
228         GET_RS2D(%f2)
229         fmuld   %f2,%f0,%f0
230         WEIRDD(%f0)
231         SET_RDD(%f0)
232         RESTORE_FP_REGS
233         RESTORE_STATE
234         jmp     %l2
235          rett   %l2+4
236
237
238 .align 8
239 divlut:
240 .double 1.0,0.8,0.666666,0.571428
241
242 sqrtlut:
243 .double 1.414214,1.264911,1.154701,1.069045
244 .double 1.0,0.894427,0.816497,0.755929
245
246 zero:   .double 0.0
247 nzero:  .double -0.0
248 half:   .double 0.5
249 two:    .double 2.0
250 three:  .double 3.0
251 nan:    .double nan
252 infty:  .double inf
253 ninfty: .double -inf
254
255 recip_asm:
256         std     %f0,[%l6+120]   ! stow input
257         ld      [%l6+120],%l3   ! l3 = MSW
258         srl     %l3,20,%l4      ! l4 = {sign,exp}
259         andn    %l4,0x800,%l5   ! l5 = exp
260 !       tst     %l5             ! denorm?
261 !       be      recip_asm_denorm!
262 !       cmp     %l5,0x7FF       ! inf?
263 !       be      recip_asm_inf   !
264         sub     %l5,2046,%l5    !
265         neg     %l5             ! l5 = -exp
266         and     %l4,0x800,%l2   ! l2 = sign
267         or      %l5,%l2,%l4     ! l4 = {sign,-exp}
268         sll     %l4,20,%l4      ! l4 = MSW
269         st      %l4,[%l6+112]   ! 
270         st      %g0,[%l6+116]   ! sp+112 = the exponent of the approx
271         srl     %l3,15,%l3      !
272         and     %l3,0x18,%l3    ! l3 = offset into LUT
273         set     divlut,%l4      !
274         ldd     [%l3+%l4],%f0   ! f0 = mantissa of approx
275         ldd     [%l6+112],%f2   ! f2 = exponent of approx
276         fmuld   %f0,%f2,%f0     ! f0 = approx
277         ldd     [%l6+120],%f2   ! f2 = b
278         sethi   %hi(two),%l3    ! f4 = two
279         ldd     [%l3+%lo(two)],%f4
280
281         fmuld   %f0,%f2,%f6     ! f6 = x*b
282         fsubd   %f4,%f6,%f6     ! f6 = 2-x*b
283         fmuld   %f0,%f6,%f0     ! f0 = x*(2-x*b)        
284
285         fmuld   %f0,%f2,%f6     ! f6 = x*b
286         fsubd   %f4,%f6,%f6     ! f6 = 2-x*b
287         fmuld   %f0,%f6,%f0     ! f0 = x*(2-x*b)        
288
289         fmuld   %f0,%f2,%f6     ! f6 = x*b
290         fsubd   %f4,%f6,%f6     ! f6 = 2-x*b
291         fmuld   %f0,%f6,%f0     ! f0 = x*(2-x*b)        
292
293         fmuld   %f0,%f2,%f6     ! f6 = x*b
294         fsubd   %f4,%f6,%f6     ! f6 = 2-x*b
295         fmuld   %f0,%f6,%f0     ! f0 = x*(2-x*b)        
296
297         fmuld   %f0,%f2,%f6     ! f6 = x*b
298         fsubd   %f4,%f6,%f6     ! f6 = 2-x*b
299         fmuld   %f0,%f6,%f0     ! f0 = x*(2-x*b)        
300
301         fmuld   %f0,%f2,%f6     ! f6 = x*b
302         fsubd   %f4,%f6,%f6     ! f6 = 2-x*b
303         fmuld   %f0,%f6,%f0     ! f0 = x*(2-x*b)        
304 recip_asm_out:
305         jmp     %l7+8
306          nop
307
308 recip_asm_inf:
309         b       giveup
310         cmp     %l3,-1          ! nan?
311         sethi   %hi(nan),%l2
312         be      recip_asm_out   ! if so, result = nan
313          ldd    [%l2+%lo(nan)],%f0
314         cmp     %l4,0x000       ! positive inf?
315         sethi   %hi(zero),%l2   !
316         be      recip_asm_out   ! if so, result = zero
317          ldd    [%l2+%lo(zero)],%f0
318         sethi   %hi(nzero),%l2  ! else, result = -zero
319         ldd     [%l2+%lo(nzero)],%f0
320         b,a     recip_asm_out
321
322 recip_asm_denorm:
323         b       giveup
324         cmp     %l4,0x000       ! positive denorm?
325         sethi   %hi(infty),%l2  !
326         be      recip_asm_out   ! if so, result = inf
327          ldd    [%l2+%lo(infty)],%f0
328         sethi   %hi(ninfty),%l2 ! else, result = -inf
329         ldd     [%l2+%lo(ninfty)],%f0
330         b,a     recip_asm_out
331         
332 recip_sqrt_asm:
333         std     %f0,[%l6+120]   ! stow input
334         ld      [%l6+120],%l3   ! l3 = MSW
335         srl     %l3,20,%l4      ! l4 = {sign,exp}
336         andn    %l4,0x800,%l5   ! l5 = exp
337 !       tst     %l5             ! denorm?
338 !       be      recip_sqrt_asm_denorm
339 !       cmp     %l5,0x7FF       ! inf?
340 !       be      recip_sqrt_asm_inf      !
341         btst    0x800,%l4       ! negative?
342         sethi   %hi(nan),%l2    !
343         bne     recip_sqrt_asm_out      ! if so, result = NaN
344          ldd    [%l2+%lo(nan)],%f0
345         sub     %l5,3069,%l5    !
346         neg     %l5             ! l5 = -exp
347         srl     %l5,1,%l5       ! l5 = -exp/2
348         sll     %l5,20,%l4      ! l4 = MSW (sign guaranteed to be 0)
349         st      %l4,[%l6+112]   ! 
350         st      %g0,[%l6+116]   ! sp+112 = the exponent of the approx
351         srl     %l3,15,%l3      !
352         and     %l3,0x38,%l3    ! l3 = offset into LUT
353         set     sqrtlut,%l4     !
354         ldd     [%l3+%l4],%f0   ! f0 = mantissa of approx
355         ldd     [%l6+112],%f2   ! f2 = exponent of approx
356         fmuld   %f0,%f2,%f0     ! f0 = approx
357         ldd     [%l6+120],%f2   ! f2 = b
358         sethi   %hi(three),%l3  ! f4 = three
359         ldd     [%l3+%lo(three)],%f4
360         sethi   %hi(half),%l3   ! f8 = half
361         ldd     [%l3+%lo(half)],%f8
362
363         fmuld   %f0,%f2,%f6     ! f6 = x*b
364         fmuld   %f0,%f6,%f6     ! f6 = x*x*b
365         fsubd   %f4,%f6,%f6     ! f6 = 3-x*x*b
366         fmuld   %f0,%f6,%f0     ! f0 = x*(3-x*x*b)
367         fmuld   %f0,%f8,%f0     ! f0 = 0.5*x*(3-x*x*b)
368
369         fmuld   %f0,%f2,%f6     ! f6 = x*b
370         fmuld   %f0,%f6,%f6     ! f6 = x*x*b
371         fsubd   %f4,%f6,%f6     ! f6 = 3-x*x*b
372         fmuld   %f0,%f6,%f0     ! f0 = x*(3-x*x*b)
373         fmuld   %f0,%f8,%f0     ! f0 = 0.5*x*(3-x*x*b)
374
375         fmuld   %f0,%f2,%f6     ! f6 = x*b
376         fmuld   %f0,%f6,%f6     ! f6 = x*x*b
377         fsubd   %f4,%f6,%f6     ! f6 = 3-x*x*b
378         fmuld   %f0,%f6,%f0     ! f0 = x*(3-x*x*b)
379         fmuld   %f0,%f8,%f0     ! f0 = 0.5*x*(3-x*x*b)
380
381         fmuld   %f0,%f2,%f6     ! f6 = x*b
382         fmuld   %f0,%f6,%f6     ! f6 = x*x*b
383         fsubd   %f4,%f6,%f6     ! f6 = 3-x*x*b
384         fmuld   %f0,%f6,%f0     ! f0 = x*(3-x*x*b)
385         fmuld   %f0,%f8,%f0     ! f0 = 0.5*x*(3-x*x*b)
386
387         fmuld   %f0,%f2,%f6     ! f6 = x*b
388         fmuld   %f0,%f6,%f6     ! f6 = x*x*b
389         fsubd   %f4,%f6,%f6     ! f6 = 3-x*x*b
390         fmuld   %f0,%f6,%f0     ! f0 = x*(3-x*x*b)
391         fmuld   %f0,%f8,%f0     ! f0 = 0.5*x*(3-x*x*b)
392
393         fmuld   %f0,%f2,%f6     ! f6 = x*b
394         fmuld   %f0,%f6,%f6     ! f6 = x*x*b
395         fsubd   %f4,%f6,%f6     ! f6 = 3-x*x*b
396         fmuld   %f0,%f6,%f0     ! f0 = x*(3-x*x*b)
397         fmuld   %f0,%f8,%f0     ! f0 = 0.5*x*(3-x*x*b)
398 recip_sqrt_asm_out:
399         jmp     %l7+8
400          nop
401
402 recip_sqrt_asm_inf:
403         b       giveup
404         cmp     %l3,-1          ! nan?
405         sethi   %hi(nan),%l2
406         be      recip_asm_out   ! if so, result = nan
407          ldd    [%l2+%lo(nan)],%f0
408         cmp     %l4,0x000       ! positive inf?
409         sethi   %hi(zero),%l2   !
410         be      recip_sqrt_asm_out      ! if so, result = zero
411          ldd    [%l2+%lo(zero)],%f0
412         sethi   %hi(nan),%l2    ! else, result = nan
413         ldd     [%l2+%lo(nan)],%f0
414         b,a     recip_sqrt_asm_out
415
416 recip_sqrt_asm_denorm:
417         b       giveup
418         cmp     %l4,0x000       ! positive denorm?
419         sethi   %hi(infty),%l2  !
420         be      recip_sqrt_asm_out      ! if so, result = inf
421          ldd    [%l2+%lo(infty)],%f0
422         sethi   %hi(nan),%l2    ! else, result = nan
423         ldd     [%l2+%lo(nan)],%f0
424         b,a     recip_sqrt_asm_out
425         
426 /*
427
428 // d2i and i2d are just wrappers to get/set the bits of a float
429
430 double recip(double b)
431 {
432   uint64_t i = d2i(b);
433   uint64_t i2 = ((2046-((i>>52)&~0x800)) | (i>>52)&0x800) << 52;
434   uint64_t i3 = (i >> 50) & 3;
435   static const double divlut[4] = {1.0,0.8,0.666,0.571};
436   double x = i2d(i2)*divlut[i3];
437
438   x = x*(2.0-b*x);
439   x = x*(2.0-b*x);
440   x = x*(2.0-b*x);
441   x = x*(2.0-b*x);
442   x = x*(2.0-b*x);
443
444   return x;
445 }
446
447 double recip_sqrt(double b)
448 {
449   uint64_t i = d2i(b);
450   uint64_t i2 = ((3069-((i>>52)&~0x800))>>1 | (i>>52)&0x800) << 52;
451   uint64_t i3 = (i >> 50) & 7;
452   double x = i2d(i2);
453
454   static const double sqrtlut[8] = {1.4142,1.264,1.155,1.069, 1.0,0.894,0.816,0.756};
455   x *= sqrtlut[i3];
456
457   x = 0.5*x*(3.0-b*x*x);
458   x = 0.5*x*(3.0-b*x*x);
459   x = 0.5*x*(3.0-b*x*x);
460   x = 0.5*x*(3.0-b*x*x);
461   x = 0.5*x*(3.0-b*x*x);
462
463   return x;
464 }
465 */