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
|
/* Copyright (c) 2002, Reiner Patommel
Copyright (c) 2006 Dmitry Xmelkov
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in
the documentation and/or other materials provided with the
distribution.
* Neither the name of the copyright holders nor the names of
contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE. */
/* $Id: fp_arccos.S,v 1.1 2007/01/14 14:52:41 dmix Exp $ */
#include "fp32def.h"
#include "asmdef.h"
/* flt40_t __fp_arccos (float x);
This is an Arccos internal function. Arg must: 0 <= x <= 1.
*/
#define FL_1 0x3f800000 /* +1.0 */
#define HI40_PI_2 0x3fc90fda /* high 4 bytes of Pi/2 */
#define LO40_PI_2 0xa2 /* lowest byte of Pi/2 */
#define rC0 r16
#define rC1 r17
#define rC2 YL
#define rC3 YH
ENTRY __fp_arccos
#if defined(__AVR_HAVE_MOVW__) && __AVR_HAVE_MOVW__
; push
push rC3
push rC2
push rC1
push rC0
; calculate: sqrt(1-A), (C0 + A*(C1 + ...))
movw rC0, rA0
movw rC2, rA2
ldi ZL, lo8(.L_table)
ldi ZH, hi8(.L_table)
rcall _U(__fp_powser)
rcall _U(__fp_round)
movw rB0, rC0
movw rB2, rC2
movw rC0, rA0
movw rC2, rA2
ldi rA0, lo8(FL_1)
ldi rA1, hi8(FL_1)
ldi rA2, hlo8(FL_1)
ldi rA3, hhi8(FL_1)
rcall _U(__subsf3)
rcall _U(sqrt)
movw rB0, rC0
movw rB2, rC2
; restore and multiply
pop rC0
pop rC1
pop rC2
pop rC3
rjmp _U(__mulsf3x)
#else /* to __AVR_HAVE_MOVW__ */
; save A
push rA3
push rA2
push rA1
push rA0
; calculate: C0 + A*(C1 + ...)
ldi ZL, lo8(.L_table)
ldi ZH, hi8(.L_table)
rcall _U(__fp_powser)
rcall _U(__fp_round)
; restore A
pop rB0
pop rB1
pop rB2
pop rB3
; save calculation result
push rA3
push rA2
push rA1
push rA0
; sqrt(1-A)
ldi rA0, lo8(FL_1)
ldi rA1, hi8(FL_1)
ldi rA2, hlo8(FL_1)
ldi rA3, hhi8(FL_1)
rcall _U(__subsf3)
rcall _U(sqrt)
; multiply
pop rB0
pop rB1
pop rB2
pop rB3
rjmp _U(__mulsf3x)
#endif
ENDFUNC
PGM_SECTION
.L_table:
.byte 7
.byte 0x2c,0x7a,0xa5,0xba ; -0.0012624911
.byte 0x6c,0xc5,0x90,0xda,0x3b ; 0.0066700901
.byte 0x93,0x65,0xfc,0x8b,0xbc ; -0.0170881256
.byte 0x53,0xf8,0x10,0xfd,0x3c ; 0.0308918810
.byte 0x56,0x92,0x83,0x4d,0xbd ; -0.0501743046
.byte 0x87,0x9d,0x3a,0xb6,0x3d ; 0.0889789874
.byte 0xcb,0xc9,0xbf,0x5b,0xbe ; -0.2145988016
.byte 0x73,0xda,0x0f,0xc9,0x3f ; 1.5707963050
.end
|
