1 /*
2 ** Copyright 2003-2010, VisualOn, Inc.
3 **
4 ** Licensed under the Apache License, Version 2.0 (the "License");
5 ** you may not use this file except in compliance with the License.
6 ** You may obtain a copy of the License at
7 **
8 ** http://www.apache.org/licenses/LICENSE-2.0
9 **
10 ** Unless required by applicable law or agreed to in writing, software
11 ** distributed under the License is distributed on an "AS IS" BASIS,
12 ** WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 ** See the License for the specific language governing permissions and
14 ** limitations under the License.
15 */
16
17 /*___________________________________________________________________________
18 | |
19 | This file contains mathematic operations in fixed point. |
20 | |
21 | Isqrt() : inverse square root (16 bits precision). |
22 | Pow2() : 2^x (16 bits precision). |
23 | Log2() : log2 (16 bits precision). |
24 | Dot_product() : scalar product of <x[],y[]> |
25 | |
26 | These operations are not standard double precision operations. |
27 | They are used where low complexity is important and the full 32 bits |
28 | precision is not necessary. For example, the function Div_32() has a |
29 | 24 bits precision which is enough for our purposes. |
30 | |
31 | In this file, the values use theses representations: |
32 | |
33 | Word32 L_32 : standard signed 32 bits format |
34 | Word16 hi, lo : L_32 = hi<<16 + lo<<1 (DPF - Double Precision Format) |
35 | Word32 frac, Word16 exp : L_32 = frac << exp-31 (normalised format) |
36 | Word16 int, frac : L_32 = int.frac (fractional format) |
37 |___________________________________________________________________________|
38 */
39 #include "typedef.h"
40 #include "basic_op.h"
41 #include "math_op.h"
42
43 /*___________________________________________________________________________
44 | |
45 | Function Name : Isqrt |
46 | |
47 | Compute 1/sqrt(L_x). |
48 | if L_x is negative or zero, result is 1 (7fffffff). |
49 |---------------------------------------------------------------------------|
50 | Algorithm: |
51 | |
52 | 1- Normalization of L_x. |
53 | 2- call Isqrt_n(L_x, exponant) |
54 | 3- L_y = L_x << exponant |
55 |___________________________________________________________________________|
56 */
Isqrt(Word32 L_x)57 Word32 Isqrt( /* (o) Q31 : output value (range: 0<=val<1) */
58 Word32 L_x /* (i) Q0 : input value (range: 0<=val<=7fffffff) */
59 )
60 {
61 Word16 exp;
62 Word32 L_y;
63 exp = norm_l(L_x);
64 L_x = (L_x << exp); /* L_x is normalized */
65 exp = (31 - exp);
66 Isqrt_n(&L_x, &exp);
67 L_y = (L_x << exp); /* denormalization */
68 return (L_y);
69 }
70
71 /*___________________________________________________________________________
72 | |
73 | Function Name : Isqrt_n |
74 | |
75 | Compute 1/sqrt(value). |
76 | if value is negative or zero, result is 1 (frac=7fffffff, exp=0). |
77 |---------------------------------------------------------------------------|
78 | Algorithm: |
79 | |
80 | The function 1/sqrt(value) is approximated by a table and linear |
81 | interpolation. |
82 | |
83 | 1- If exponant is odd then shift fraction right once. |
84 | 2- exponant = -((exponant-1)>>1) |
85 | 3- i = bit25-b30 of fraction, 16 <= i <= 63 ->because of normalization. |
86 | 4- a = bit10-b24 |
87 | 5- i -=16 |
88 | 6- fraction = table[i]<<16 - (table[i] - table[i+1]) * a * 2 |
89 |___________________________________________________________________________|
90 */
91 static Word16 table_isqrt[49] =
92 {
93 32767, 31790, 30894, 30070, 29309, 28602, 27945, 27330, 26755, 26214,
94 25705, 25225, 24770, 24339, 23930, 23541, 23170, 22817, 22479, 22155,
95 21845, 21548, 21263, 20988, 20724, 20470, 20225, 19988, 19760, 19539,
96 19326, 19119, 18919, 18725, 18536, 18354, 18176, 18004, 17837, 17674,
97 17515, 17361, 17211, 17064, 16921, 16782, 16646, 16514, 16384
98 };
99
Isqrt_n(Word32 * frac,Word16 * exp)100 void Isqrt_n(
101 Word32 * frac, /* (i/o) Q31: normalized value (1.0 < frac <= 0.5) */
102 Word16 * exp /* (i/o) : exponent (value = frac x 2^exponent) */
103 )
104 {
105 Word16 i, a, tmp;
106
107 if (*frac <= (Word32) 0)
108 {
109 *exp = 0;
110 *frac = 0x7fffffffL;
111 return;
112 }
113
114 if((*exp & 1) == 1) /*If exponant odd -> shift right */
115 *frac = (*frac) >> 1;
116
117 *exp = negate((*exp - 1) >> 1);
118
119 *frac = (*frac >> 9);
120 i = extract_h(*frac); /* Extract b25-b31 */
121 *frac = (*frac >> 1);
122 a = (Word16)(*frac); /* Extract b10-b24 */
123 a = (Word16) (a & (Word16) 0x7fff);
124 i -= 16;
125 *frac = L_deposit_h(table_isqrt[i]); /* table[i] << 16 */
126 tmp = vo_sub(table_isqrt[i], table_isqrt[i + 1]); /* table[i] - table[i+1]) */
127 *frac = vo_L_msu(*frac, tmp, a); /* frac -= tmp*a*2 */
128
129 return;
130 }
131
132 /*___________________________________________________________________________
133 | |
134 | Function Name : Pow2() |
135 | |
136 | L_x = pow(2.0, exponant.fraction) (exponant = interger part) |
137 | = pow(2.0, 0.fraction) << exponant |
138 |---------------------------------------------------------------------------|
139 | Algorithm: |
140 | |
141 | The function Pow2(L_x) is approximated by a table and linear |
142 | interpolation. |
143 | |
144 | 1- i = bit10-b15 of fraction, 0 <= i <= 31 |
145 | 2- a = bit0-b9 of fraction |
146 | 3- L_x = table[i]<<16 - (table[i] - table[i+1]) * a * 2 |
147 | 4- L_x = L_x >> (30-exponant) (with rounding) |
148 |___________________________________________________________________________|
149 */
150 static Word16 table_pow2[33] =
151 {
152 16384, 16743, 17109, 17484, 17867, 18258, 18658, 19066, 19484, 19911,
153 20347, 20792, 21247, 21713, 22188, 22674, 23170, 23678, 24196, 24726,
154 25268, 25821, 26386, 26964, 27554, 28158, 28774, 29405, 30048, 30706,
155 31379, 32066, 32767
156 };
157
Pow2(Word16 exponant,Word16 fraction)158 Word32 Pow2( /* (o) Q0 : result (range: 0<=val<=0x7fffffff) */
159 Word16 exponant, /* (i) Q0 : Integer part. (range: 0<=val<=30) */
160 Word16 fraction /* (i) Q15 : Fractionnal part. (range: 0.0<=val<1.0) */
161 )
162 {
163 Word16 exp, i, a, tmp;
164 Word32 L_x;
165
166 L_x = vo_L_mult(fraction, 32); /* L_x = fraction<<6 */
167 i = extract_h(L_x); /* Extract b10-b16 of fraction */
168 L_x =L_x >> 1;
169 a = (Word16)(L_x); /* Extract b0-b9 of fraction */
170 a = (Word16) (a & (Word16) 0x7fff);
171
172 L_x = L_deposit_h(table_pow2[i]); /* table[i] << 16 */
173 tmp = vo_sub(table_pow2[i], table_pow2[i + 1]); /* table[i] - table[i+1] */
174 L_x -= (tmp * a)<<1; /* L_x -= tmp*a*2 */
175
176 exp = vo_sub(30, exponant);
177 L_x = vo_L_shr_r(L_x, exp);
178
179 return (L_x);
180 }
181
182 /*___________________________________________________________________________
183 | |
184 | Function Name : Dot_product12() |
185 | |
186 | Compute scalar product of <x[],y[]> using accumulator. |
187 | |
188 | The result is normalized (in Q31) with exponent (0..30). |
189 |---------------------------------------------------------------------------|
190 | Algorithm: |
191 | |
192 | dot_product = sum(x[i]*y[i]) i=0..N-1 |
193 |___________________________________________________________________________|
194 */
195
Dot_product12(Word16 x[],Word16 y[],Word16 lg,Word16 * exp)196 Word32 Dot_product12( /* (o) Q31: normalized result (1 < val <= -1) */
197 Word16 x[], /* (i) 12bits: x vector */
198 Word16 y[], /* (i) 12bits: y vector */
199 Word16 lg, /* (i) : vector length */
200 Word16 * exp /* (o) : exponent of result (0..+30) */
201 )
202 {
203 Word16 sft;
204 Word32 i, L_sum;
205 L_sum = 0;
206 for (i = 0; i < lg; i++)
207 {
208 Word32 tmp = (Word32) x[i] * (Word32) y[i];
209 if (tmp == (Word32) 0x40000000L) {
210 tmp = MAX_32;
211 }
212 L_sum = L_add(L_sum, tmp);
213 }
214 L_sum = L_shl2(L_sum, 1);
215 L_sum = L_add(L_sum, 1);
216 /* Normalize acc in Q31 */
217 sft = norm_l(L_sum);
218 L_sum = L_sum << sft;
219 *exp = 30 - sft; /* exponent = 0..30 */
220 return (L_sum);
221
222 }
223
224
225