/* ** Copyright 2003-2010, VisualOn, Inc. ** ** Licensed under the Apache License, Version 2.0 (the "License"); ** you may not use this file except in compliance with the License. ** You may obtain a copy of the License at ** ** http://www.apache.org/licenses/LICENSE-2.0 ** ** Unless required by applicable law or agreed to in writing, software ** distributed under the License is distributed on an "AS IS" BASIS, ** WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. ** See the License for the specific language governing permissions and ** limitations under the License. */ /*___________________________________________________________________________ | | | This file contains mathematic operations in fixed point. | | | | Isqrt() : inverse square root (16 bits precision). | | Pow2() : 2^x (16 bits precision). | | Log2() : log2 (16 bits precision). | | Dot_product() : scalar product of | | | | These operations are not standard double precision operations. | | They are used where low complexity is important and the full 32 bits | | precision is not necessary. For example, the function Div_32() has a | | 24 bits precision which is enough for our purposes. | | | | In this file, the values use theses representations: | | | | Word32 L_32 : standard signed 32 bits format | | Word16 hi, lo : L_32 = hi<<16 + lo<<1 (DPF - Double Precision Format) | | Word32 frac, Word16 exp : L_32 = frac << exp-31 (normalised format) | | Word16 int, frac : L_32 = int.frac (fractional format) | |___________________________________________________________________________| */ #include "typedef.h" #include "basic_op.h" #include "math_op.h" /*___________________________________________________________________________ | | | Function Name : Isqrt | | | | Compute 1/sqrt(L_x). | | if L_x is negative or zero, result is 1 (7fffffff). | |---------------------------------------------------------------------------| | Algorithm: | | | | 1- Normalization of L_x. | | 2- call Isqrt_n(L_x, exponant) | | 3- L_y = L_x << exponant | |___________________________________________________________________________| */ Word32 Isqrt( /* (o) Q31 : output value (range: 0<=val<1) */ Word32 L_x /* (i) Q0 : input value (range: 0<=val<=7fffffff) */ ) { Word16 exp; Word32 L_y; exp = norm_l(L_x); L_x = (L_x << exp); /* L_x is normalized */ exp = (31 - exp); Isqrt_n(&L_x, &exp); L_y = (L_x << exp); /* denormalization */ return (L_y); } /*___________________________________________________________________________ | | | Function Name : Isqrt_n | | | | Compute 1/sqrt(value). | | if value is negative or zero, result is 1 (frac=7fffffff, exp=0). | |---------------------------------------------------------------------------| | Algorithm: | | | | The function 1/sqrt(value) is approximated by a table and linear | | interpolation. | | | | 1- If exponant is odd then shift fraction right once. | | 2- exponant = -((exponant-1)>>1) | | 3- i = bit25-b30 of fraction, 16 <= i <= 63 ->because of normalization. | | 4- a = bit10-b24 | | 5- i -=16 | | 6- fraction = table[i]<<16 - (table[i] - table[i+1]) * a * 2 | |___________________________________________________________________________| */ static Word16 table_isqrt[49] = { 32767, 31790, 30894, 30070, 29309, 28602, 27945, 27330, 26755, 26214, 25705, 25225, 24770, 24339, 23930, 23541, 23170, 22817, 22479, 22155, 21845, 21548, 21263, 20988, 20724, 20470, 20225, 19988, 19760, 19539, 19326, 19119, 18919, 18725, 18536, 18354, 18176, 18004, 17837, 17674, 17515, 17361, 17211, 17064, 16921, 16782, 16646, 16514, 16384 }; void Isqrt_n( Word32 * frac, /* (i/o) Q31: normalized value (1.0 < frac <= 0.5) */ Word16 * exp /* (i/o) : exponent (value = frac x 2^exponent) */ ) { Word16 i, a, tmp; if (*frac <= (Word32) 0) { *exp = 0; *frac = 0x7fffffffL; return; } if((*exp & 1) == 1) /*If exponant odd -> shift right */ *frac = (*frac) >> 1; *exp = negate((*exp - 1) >> 1); *frac = (*frac >> 9); i = extract_h(*frac); /* Extract b25-b31 */ *frac = (*frac >> 1); a = (Word16)(*frac); /* Extract b10-b24 */ a = (Word16) (a & (Word16) 0x7fff); i -= 16; *frac = L_deposit_h(table_isqrt[i]); /* table[i] << 16 */ tmp = vo_sub(table_isqrt[i], table_isqrt[i + 1]); /* table[i] - table[i+1]) */ *frac = vo_L_msu(*frac, tmp, a); /* frac -= tmp*a*2 */ return; } /*___________________________________________________________________________ | | | Function Name : Pow2() | | | | L_x = pow(2.0, exponant.fraction) (exponant = interger part) | | = pow(2.0, 0.fraction) << exponant | |---------------------------------------------------------------------------| | Algorithm: | | | | The function Pow2(L_x) is approximated by a table and linear | | interpolation. | | | | 1- i = bit10-b15 of fraction, 0 <= i <= 31 | | 2- a = bit0-b9 of fraction | | 3- L_x = table[i]<<16 - (table[i] - table[i+1]) * a * 2 | | 4- L_x = L_x >> (30-exponant) (with rounding) | |___________________________________________________________________________| */ static Word16 table_pow2[33] = { 16384, 16743, 17109, 17484, 17867, 18258, 18658, 19066, 19484, 19911, 20347, 20792, 21247, 21713, 22188, 22674, 23170, 23678, 24196, 24726, 25268, 25821, 26386, 26964, 27554, 28158, 28774, 29405, 30048, 30706, 31379, 32066, 32767 }; Word32 Pow2( /* (o) Q0 : result (range: 0<=val<=0x7fffffff) */ Word16 exponant, /* (i) Q0 : Integer part. (range: 0<=val<=30) */ Word16 fraction /* (i) Q15 : Fractionnal part. (range: 0.0<=val<1.0) */ ) { Word16 exp, i, a, tmp; Word32 L_x; L_x = vo_L_mult(fraction, 32); /* L_x = fraction<<6 */ i = extract_h(L_x); /* Extract b10-b16 of fraction */ L_x =L_x >> 1; a = (Word16)(L_x); /* Extract b0-b9 of fraction */ a = (Word16) (a & (Word16) 0x7fff); L_x = L_deposit_h(table_pow2[i]); /* table[i] << 16 */ tmp = vo_sub(table_pow2[i], table_pow2[i + 1]); /* table[i] - table[i+1] */ L_x -= (tmp * a)<<1; /* L_x -= tmp*a*2 */ exp = vo_sub(30, exponant); L_x = vo_L_shr_r(L_x, exp); return (L_x); } /*___________________________________________________________________________ | | | Function Name : Dot_product12() | | | | Compute scalar product of using accumulator. | | | | The result is normalized (in Q31) with exponent (0..30). | |---------------------------------------------------------------------------| | Algorithm: | | | | dot_product = sum(x[i]*y[i]) i=0..N-1 | |___________________________________________________________________________| */ Word32 Dot_product12( /* (o) Q31: normalized result (1 < val <= -1) */ Word16 x[], /* (i) 12bits: x vector */ Word16 y[], /* (i) 12bits: y vector */ Word16 lg, /* (i) : vector length */ Word16 * exp /* (o) : exponent of result (0..+30) */ ) { Word16 sft; Word32 i, L_sum; L_sum = 0; for (i = 0; i < lg; i++) { Word32 tmp = (Word32) x[i] * (Word32) y[i]; if (tmp == (Word32) 0x40000000L) { tmp = MAX_32; } L_sum = L_add(L_sum, tmp); } L_sum = L_shl2(L_sum, 1); L_sum = L_add(L_sum, 1); /* Normalize acc in Q31 */ sft = norm_l(L_sum); L_sum = L_sum << sft; *exp = 30 - sft; /* exponent = 0..30 */ return (L_sum); }