source: svn/trunk/newcon3bcm2_21bu/magnum/commonutils/mth/bmth_fix.c @ 36

Last change on this file since 36 was 2, checked in by jglee, 11 years ago

first commit
  • Property svn:executable set to *
File size: 13.9 KB
Line 
1/***************************************************************************
2 *     Copyright (c) 2003-2011, Broadcom Corporation
3 *     All Rights Reserved
4 *     Confidential Property of Broadcom Corporation
5 *
6 *  THIS SOFTWARE MAY ONLY BE USED SUBJECT TO AN EXECUTED SOFTWARE LICENSE
7 *  AGREEMENT  BETWEEN THE USER AND BROADCOM.  YOU HAVE NO RIGHT TO USE OR
8 *  EXPLOIT THIS MATERIAL EXCEPT SUBJECT TO THE TERMS OF SUCH AN AGREEMENT.
9 *
10 * $brcm_Workfile: bmth_fix.c $
11 * $brcm_Revision: Hydra_Software_Devel/12 $
12 * $brcm_Date: 3/17/11 4:31p $
13 *
14 * Module Description:
15 *
16 * Revision History:
17 *
18 * $brcm_Log: /magnum/commonutils/mth/bmth_fix.c $
19 *
20 * Hydra_Software_Devel/12   3/17/11 4:31p albertl
21 * SW7405-5156: Added back int32_t cast which allows for proper sign
22 * propogation when casting to int64_t.
23 *
24 * Hydra_Software_Devel/11   2/14/11 4:54p albertl
25 * SW35230-2975: Changed  multiplication to really use 64-bit  *
26 * intermediate.
27 *
28 * Hydra_Software_Devel/9   8/26/10 7:46p albertl
29 * SW7405-4556, SW7405-4515: Changed multiplication to use 64-bit
30 * intermediate.  Added fix to float macro.
31 *
32 * Hydra_Software_Devel/8   2/17/09 6:48p albertl
33 * PR51613: Updated to correct naming conventions.
34 *
35 * Hydra_Software_Devel/7   11/14/08 6:12p albertl
36 * PR47814:  Added debug code to check for fixed multiplication overflows.
37 *
38 * Hydra_Software_Devel/6   8/4/08 1:57p tdo
39 * PR45389: Make inline user definable because it is not compile for some
40 * std.
41 *
42 * Hydra_Software_Devel/6   8/4/08 1:46p tdo
43 * PR45389: Make inline user definable because it is not compile for some
44 * std.
45 *
46 * Hydra_Software_Devel/6   8/4/08 1:43p tdo
47 * PR45389: Make inline user definable because it is not compile for some
48 * std.
49 *
50 * Hydra_Software_Devel/5   2/8/08 4:46p tdo
51 * PR37722: Use inline function to optimize VDC math related performance
52 *
53 * Hydra_Software_Devel/4   9/20/07 1:25a albertl
54 * PR35135:  Cleaned up color space matrices and changed them to use same
55 * macro system.  Added color space conversion functionality to graphics
56 * windows.
57 *
58 * Hydra_Software_Devel/3   8/9/07 12:27p tdo
59 * PR33574: Convert more macro defines into functions to avoid compiling
60 * errors in WinCE build
61 *
62 * Hydra_Software_Devel/2   8/3/07 5:18p tdo
63 * PR33574: Make BMTH_FIX_SIGNED_MUL into function call to eliminate
64 * compiling errors in WinCE platform build
65 *
66 * Hydra_Software_Devel/1   2/22/06 6:11p albertl
67 * PR18913:  Initial revision of BMTH_FIX fixed point math library.
68 *
69 ***************************************************************************/
70#include "bmth_fix.h"
71#include "bstd.h"
72
73/* sin magnitude lookup table in signed 2.10 notation */
74static const int32_t s_lFixSinMagTable[] =
75{
76        0x0000, 0x000d, 0x0019, 0x0026, 0x0033, 0x003f, 0x004c, 0x0059,
77        0x0065, 0x0072, 0x007e, 0x008b, 0x0097, 0x00a4, 0x00b0, 0x00bd,
78        0x00c9, 0x00d6, 0x00e2, 0x00ee, 0x00fb, 0x0107, 0x0113, 0x011f,
79        0x012b, 0x0138, 0x0144, 0x0150, 0x015b, 0x0167, 0x0173, 0x017f,
80        0x018b, 0x0196, 0x01a2, 0x01ad, 0x01b9, 0x01c4, 0x01d0, 0x01db,
81        0x01e6, 0x01f1, 0x01fc, 0x0207, 0x0212, 0x021d, 0x0228, 0x0232,
82        0x023d, 0x0247, 0x0252, 0x025c, 0x0266, 0x0270, 0x027a, 0x0284,
83        0x028e, 0x0297, 0x02a1, 0x02ab, 0x02b4, 0x02bd, 0x02c6, 0x02cf,
84        0x02d8, 0x02e1, 0x02ea, 0x02f3, 0x02fb, 0x0303, 0x030c, 0x0314,
85        0x031c, 0x0324, 0x032c, 0x0333, 0x033b, 0x0342, 0x0349, 0x0351,
86        0x0358, 0x035f, 0x0365, 0x036c, 0x0372, 0x0379, 0x037f, 0x0385,
87        0x038b, 0x0391, 0x0396, 0x039c, 0x03a1, 0x03a7, 0x03ac, 0x03b1,
88        0x03b6, 0x03ba, 0x03bf, 0x03c3, 0x03c7, 0x03cb, 0x03cf, 0x03d3,
89        0x03d7, 0x03da, 0x03de, 0x03e1, 0x03e4, 0x03e7, 0x03e9, 0x03ec,
90        0x03ee, 0x03f1, 0x03f3, 0x03f5, 0x03f6, 0x03f8, 0x03fa, 0x03fb,
91        0x03fc, 0x03fd, 0x03fe, 0x03ff, 0x03ff, 0x0400, 0x0400, 0x0400
92};
93
94
95/*************************************************************************
96 * BMTH_FIX_SIGNED_CONVERT
97 *
98 *************************************************************************/
99BMTH_INLINE uint32_t BMTH_FIX_SIGNED_CONVERT(uint32_t x, uint32_t inint, uint32_t infract, uint32_t outint, uint32_t outfract)
100{
101        uint32_t lFixOut;
102        uint32_t ulFixInt;
103        uint32_t ulSignBit;
104
105        ulFixInt = (infract > outfract) ? (x >> (infract - outfract)) : (x << (outfract - infract));
106        if(inint > outint)
107        {
108                ulSignBit = 0;
109        }
110        else
111        {
112                if(x & BMTH_P_FIX_SIGN_BIT(inint, infract))
113                {
114                        ulSignBit = ~BMTH_P_FIX_SIGNED_MASK(inint, outfract);
115                }
116                else
117                {
118                        ulSignBit = 0;
119                }
120        }
121        lFixOut = (ulFixInt | ulSignBit) & BMTH_P_FIX_SIGNED_MASK(outint, outfract);
122
123        return lFixOut;
124}
125
126/*************************************************************************
127 * BMTH_FIX_SIGNED_MUL
128 *
129 *************************************************************************/
130BMTH_INLINE uint32_t BMTH_FIX_SIGNED_MUL(uint32_t x, uint32_t y, uint32_t xint, uint32_t xfract, uint32_t yint, uint32_t yfract, uint32_t outint, uint32_t outfract)
131{
132        int64_t lFixOut; /* 64-bit to hold intermediate multiplied value of two 32-bit values */
133
134#if 0
135        /* used to debug overflows */
136        int32_t lhi, llo;
137        int32_t lExtendedX = (int32_t)BMTH_FIX_SIGNED_CONVERT(x, xint, xfract, BMTH_P_FIX_SIGNED_MAX_BITS - xfract, xfract);
138        int32_t lExtendedY = (int32_t)BMTH_FIX_SIGNED_CONVERT(y, yint, yfract, BMTH_P_FIX_SIGNED_MAX_BITS - yfract, yfract);
139        int32_t lSignMask = BMTH_P_FIX_SIGN_BIT(BMTH_P_FIX_SIGNED_MAX_BITS, 0);
140
141        BMTH_HILO_32TO64_Mul (lExtendedX, lExtendedY, &lhi, &llo);
142
143        if (lhi)
144        {
145                if ((!(lExtendedX & lSignMask) && !(lExtendedY & lSignMask)) ||
146                        (!(llo & lSignMask) && (((lExtendedX & lSignMask) && !(lExtendedY & lSignMask)) ||
147                                                 (!(lExtendedX & lSignMask) && (lExtendedY & lSignMask)))) ||
148                        ((llo & lSignMask) && (lExtendedX & lSignMask) && (lExtendedY & lSignMask)))
149                {
150                        printf("BMTH_FIX_SIGNED_MUL OVERFLOW! x: 0x%x, y: 0x%x, lhi: 0x%x, llo: 0x%x\n", lExtendedX, lExtendedY, lhi, llo);
151                }
152        }
153
154        if(outfract > (xfract + yfract))
155        {
156                lFixOut = llo  << (outfract - (xfract + yfract));
157        }
158        else
159        {
160                lFixOut = llo >> ((xfract + yfract) - outfract);
161        }
162
163#else
164        if(outfract > (xfract + yfract))
165        {
166                lFixOut = ((int64_t)(int32_t)BMTH_FIX_SIGNED_CONVERT(x, xint, xfract, BMTH_P_FIX_SIGNED_MAX_BITS - xfract, xfract) *
167                                            (int32_t)BMTH_FIX_SIGNED_CONVERT(y, yint, yfract, BMTH_P_FIX_SIGNED_MAX_BITS - yfract, yfract))
168                                  << (outfract - (xfract + yfract));
169        }
170        else
171        {
172                lFixOut = ((int64_t)(int32_t)BMTH_FIX_SIGNED_CONVERT(x, xint, xfract, BMTH_P_FIX_SIGNED_MAX_BITS - xfract, xfract) *
173                                            (int32_t)BMTH_FIX_SIGNED_CONVERT(y, yint, yfract, BMTH_P_FIX_SIGNED_MAX_BITS - yfract, yfract))
174                                  >> ((xfract + yfract) - outfract);
175        }
176
177#endif
178
179        return (uint32_t)(lFixOut & (int32_t)BMTH_P_FIX_SIGNED_MASK(outint, outfract));
180}
181
182/*************************************************************************
183 * BMTH_FIX_LOG2
184 *
185 *************************************************************************/
186BMTH_INLINE uint32_t BMTH_FIX_LOG2(uint32_t x)
187{
188        const uint32_t bitarray[] = {0x2, 0xC, 0xF0, 0xFF00, 0xFFFF0000};
189        const uint32_t shiftarray[] = {1, 2, 4, 8, 16};
190        int32_t i = 0;
191        uint32_t log = 0;
192        for (i = 4; i >= 0; i--)
193        {
194                if (x & bitarray[i])
195                {
196                        x >>= shiftarray[i];
197                        log |= shiftarray[i];
198                }
199        }
200        return log;
201}
202
203/*************************************************************************
204 * BMTH_P_FIX_SIGNED_MININTBITS
205 *
206 *************************************************************************/
207BMTH_INLINE uint32_t BMTH_P_FIX_SIGNED_MININTBITS(uint32_t x, uint32_t intbits, uint32_t fractbits)
208{
209        uint32_t ulMinIntBits;
210        int32_t ulFixInput = BMTH_FIX_SIGNED_CONVERT(x, intbits, fractbits, BMTH_P_FIX_SIGNED_MAX_BITS, 0);
211
212        if(x & BMTH_P_FIX_SIGN_BIT(intbits, fractbits))
213        {
214                ulMinIntBits = BMTH_FIX_LOG2((uint32_t)((int32_t)ulFixInput * (-1)));
215        }
216        else
217        {
218                ulMinIntBits = BMTH_FIX_LOG2(ulFixInput) + 1;
219        }
220        return ulMinIntBits;
221}
222
223
224/*************************************************************************
225 * BMTH_FIX_SIGNED_MOD
226 *
227 *************************************************************************/
228uint32_t BMTH_FIX_SIGNED_MOD(uint32_t x, uint32_t y, uint32_t xint, uint32_t xfract, uint32_t yint, uint32_t yfract, uint32_t outint, uint32_t outfract)
229{
230        int32_t  lFixX;
231        int32_t  lFixY;
232        int32_t  lFixMod;
233        uint32_t ulTempIntBits = 0;
234        uint32_t ulTempFractBits = 0;
235
236#if 1
237        uint32_t ulTempXIntBits = 0;
238        uint32_t ulTempYIntBits = 0;
239
240        ulTempXIntBits = BMTH_P_FIX_SIGNED_MININTBITS(x, xint, xfract);
241        ulTempYIntBits = BMTH_P_FIX_SIGNED_MININTBITS(y, yint, yfract);
242        ulTempIntBits = (ulTempXIntBits > ulTempYIntBits) ? ulTempXIntBits : ulTempYIntBits;
243
244        if (ulTempIntBits < outint)
245        {
246                ulTempIntBits = outint;
247        }
248#else
249        ulTempIntBits = outint;
250#endif
251
252        ulTempFractBits = BMTH_P_FIX_SIGNED_MAX_BITS - ulTempIntBits;
253
254        /* convert to common format */
255        lFixX = BMTH_FIX_SIGNED_CONVERT(x, xint, xfract,
256                                            ulTempIntBits, ulTempFractBits);
257        lFixY = BMTH_FIX_SIGNED_CONVERT(y, yint, yfract,
258                                            ulTempIntBits, ulTempFractBits);
259        lFixMod = lFixX % lFixY;
260
261        return BMTH_FIX_SIGNED_CONVERT(lFixMod, ulTempIntBits, ulTempFractBits, outint, outfract);
262}
263
264
265/***************************************************************************
266 * {private}
267 *
268 */
269uint32_t BMTH_P_FIX_SIGNED_SIN_CALC(uint32_t x, uint32_t xint, uint32_t xfract, uint32_t sinint, uint32_t sinfract)
270{
271        uint32_t ulRadIntBits      = xint;
272        uint32_t ulRadFractBits    = xfract;
273        uint32_t ulSinMagIntBits   = BMTH_P_FIX_SIGNED_SINMAG_INT_BITS;
274        uint32_t ulSinMagFractBits = BMTH_P_FIX_SIGNED_SINMAG_FRACT_BITS;
275        uint32_t ulInterpIntBits   = BMTH_P_FIX_SIGNED_RAD_INTERP_INT_BITS;
276        uint32_t ulInterpFractBits = BMTH_P_FIX_SIGNED_RAD_INTERP_FRACT_BITS;
277
278        int32_t ulFixX = x;
279        int32_t ulFixXModPi;
280        int32_t ulFixFirstQuadX;
281        int32_t ulFixPi = BMTH_FIX_SIGNED_GET_PI(ulRadIntBits, ulRadFractBits);
282        int32_t ulFixHalfPi = ulFixPi / 2;
283
284        uint32_t ulRadShiftBits = 0;
285        uint32_t ulSinMagTableSizeBits = 0;
286        uint32_t ulTotalBits = 0;
287        uint32_t ulSinMagIdx = 0;
288        int32_t ulFixSinMag = 0;
289        int32_t ulFixSinMagNext = 0;
290
291        int32_t ulFixInterpNum = 0;
292        int32_t ulFixInterpDem = 0;
293        uint32_t ulFixInterpCoeff = 0;
294
295
296        /* convert to positive angle */
297        ulFixX = (ulFixX & BMTH_P_FIX_SIGN_BIT(ulRadIntBits, ulRadFractBits)) ?
298                         (ulFixX + (2 * ulFixPi)) : ulFixX;
299        ulFixX &= BMTH_P_FIX_SIGNED_MASK(ulRadIntBits, ulRadFractBits);
300
301        ulFixXModPi = BMTH_FIX_SIGNED_MOD(ulFixX, ulFixPi,
302                                            ulRadIntBits, ulRadFractBits,
303                                            ulRadIntBits, ulRadFractBits,
304                                            ulRadIntBits, ulRadFractBits);
305
306        /* convert to first quadrant equivalent with same sin value */
307        ulFixFirstQuadX = (ulFixXModPi <= ulFixHalfPi) ?
308                        ulFixXModPi : (ulFixPi - ulFixXModPi);
309
310        /* get sin magnitude table index */
311        ulSinMagTableSizeBits = BMTH_P_FIX_SIGNED_MININTBITS((BMTH_P_FIX_SINMAG_TABLE_SIZE - 1), BMTH_P_FIX_SIGNED_MAX_BITS, 0);
312        ulTotalBits = ulRadIntBits + ulRadFractBits + ulSinMagTableSizeBits;
313
314        if (ulTotalBits > BMTH_P_FIX_SIGNED_MAX_BITS)
315        {
316                ulRadShiftBits = ulTotalBits - BMTH_P_FIX_SIGNED_MAX_BITS;
317        }
318
319        ulSinMagIdx = ((ulFixFirstQuadX >> ulRadShiftBits) * (BMTH_P_FIX_SINMAG_TABLE_SIZE - 1)) /
320                           (ulFixHalfPi >> ulRadShiftBits);
321        ulFixSinMag = s_lFixSinMagTable[ulSinMagIdx];
322        ulFixSinMagNext = (ulSinMagIdx < (BMTH_P_FIX_SINMAG_TABLE_SIZE - 1)) ?
323                (s_lFixSinMagTable[ulSinMagIdx + 1]) : (s_lFixSinMagTable[ulSinMagIdx]);
324
325
326        ulFixInterpNum = ulFixFirstQuadX - (((ulSinMagIdx * (ulFixHalfPi >> ulRadShiftBits)) /
327                                             (BMTH_P_FIX_SINMAG_TABLE_SIZE - 1)) << ulRadShiftBits);
328        ulFixInterpDem = ulFixHalfPi / BMTH_P_FIX_SINMAG_TABLE_SIZE;
329
330
331        ulFixInterpNum = BMTH_FIX_SIGNED_CONVERT(ulFixInterpNum,
332                                                                                         ulRadIntBits, ulRadFractBits,
333                                                                                         ulInterpIntBits, ulInterpFractBits);
334
335        ulFixInterpDem = BMTH_FIX_SIGNED_CONVERT(ulFixInterpDem,
336                                                                                         ulRadIntBits, ulRadFractBits,
337                                                                                         ulInterpIntBits, ulInterpFractBits);
338
339        ulFixInterpCoeff = BMTH_FIX_SIGNED_DIV(ulFixInterpNum, ulFixInterpDem,
340                                                                           ulInterpIntBits, ulInterpFractBits,
341                                                                           ulInterpIntBits, ulInterpFractBits,
342                                                                           ulInterpIntBits, ulInterpFractBits);
343
344        ulFixSinMag = BMTH_FIX_SIGNED_CONVERT(ulFixSinMag,
345                                                                                  ulSinMagIntBits, ulSinMagFractBits,
346                                                                                  ulInterpIntBits, ulInterpFractBits);
347
348        ulFixSinMagNext = BMTH_FIX_SIGNED_CONVERT(ulFixSinMagNext,
349                                                                                          ulSinMagIntBits, ulSinMagFractBits,
350                                                                                          ulInterpIntBits, ulInterpFractBits);
351
352        return (BMTH_FIX_SIGNED_CONVERT(ulFixSinMag,
353                                                                        ulInterpIntBits, ulInterpFractBits,
354                                                                        sinint, sinfract) +
355                    BMTH_FIX_SIGNED_MUL((ulFixSinMagNext - ulFixSinMag), ulFixInterpCoeff,
356                                                            ulInterpIntBits, ulInterpFractBits,
357                                                                ulInterpIntBits, ulInterpFractBits,
358                                    sinint, sinfract)) *
359                   ((ulFixX > ulFixPi) ? -1 : 1);  /* add the sign back */
360}
361
362
363/*************************************************************************
364 * BMTH_FIX_SIGNED_COS
365 *
366 *************************************************************************/
367uint32_t BMTH_FIX_SIGNED_COS(uint32_t x, uint32_t xint, uint32_t xfract, uint32_t sinint, uint32_t sinfract)
368{
369        uint32_t ulFixX;
370        uint32_t ulRadIntBits      = BMTH_P_FIX_SIGNED_RAD_INT_BITS;
371        uint32_t ulRadFractBits    = BMTH_P_FIX_SIGNED_RAD_FRACT_BITS;
372
373        ulFixX = BMTH_FIX_SIGNED_RADTO2PI(x, xint, xfract, ulRadIntBits, ulRadFractBits);
374        ulFixX += BMTH_FIX_SIGNED_GET_HALF_PI(ulRadIntBits, ulRadFractBits);
375
376        return BMTH_P_FIX_SIGNED_SIN_CALC(ulFixX, ulRadIntBits, ulRadFractBits, sinint, sinfract);
377}
378
379
380/*************************************************************************
381 * BMTH_FIX_SIGNED_SIN
382 *
383 *************************************************************************/
384uint32_t BMTH_FIX_SIGNED_SIN(uint32_t x, uint32_t xint, uint32_t xfract, uint32_t sinint, uint32_t sinfract)
385{
386        uint32_t ulFixX;
387        uint32_t ulRadIntBits      = BMTH_P_FIX_SIGNED_RAD_INT_BITS;
388        uint32_t ulRadFractBits    = BMTH_P_FIX_SIGNED_RAD_FRACT_BITS;
389
390        ulFixX = BMTH_FIX_SIGNED_RADTO2PI(x, xint, xfract, ulRadIntBits, ulRadFractBits);
391
392        return BMTH_P_FIX_SIGNED_SIN_CALC(ulFixX, ulRadIntBits, ulRadFractBits, sinint, sinfract);
393}
394
395
396/* end of file */
Note: See TracBrowser for help on using the repository browser.