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 */ 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 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 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 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
