| 1 | typedef struct |
|---|
| 2 | { |
|---|
| 3 | UT_U32 lo; |
|---|
| 4 | UT_I32 hi; |
|---|
| 5 | } UT_I64; |
|---|
| 6 | |
|---|
| 7 | static void Mul(UT_I32 multiplicand, UT_I32 multiplier, UT_I64* result) |
|---|
| 8 | { |
|---|
| 9 | UT_U32 lo1 = multiplicand & 0x0000FFFFU; |
|---|
| 10 | UT_U32 hi1 = multiplicand >> 16; |
|---|
| 11 | UT_U32 lo2 = multiplier & 0x0000FFFFU; |
|---|
| 12 | UT_U32 hi2 = multiplier >> 16; |
|---|
| 13 | UT_U32 lo = lo1 * lo2; |
|---|
| 14 | UT_U32 i1 = lo1 * hi2; |
|---|
| 15 | UT_U32 i2 = lo2 * hi1; |
|---|
| 16 | UT_U32 hi = hi1 * hi2; |
|---|
| 17 | i1 += i2; |
|---|
| 18 | hi += (UT_U32)(i1 < i2) << 16; |
|---|
| 19 | hi += i1 >> 16; |
|---|
| 20 | i1 = i1 << 16; |
|---|
| 21 | lo += i1; |
|---|
| 22 | hi += (lo < i1); |
|---|
| 23 | result->lo = lo; |
|---|
| 24 | result->hi = hi; |
|---|
| 25 | } |
|---|
| 26 | |
|---|
| 27 | static UT_I32 Div(UT_I64 dividend, UT_U32 divisor) |
|---|
| 28 | { |
|---|
| 29 | UT_U32 r = dividend.hi; |
|---|
| 30 | UT_U32 q = 0; |
|---|
| 31 | int i = 32; |
|---|
| 32 | if (r >= divisor) return 0x7FFFFFFFU; |
|---|
| 33 | do |
|---|
| 34 | { |
|---|
| 35 | r <<= 1; |
|---|
| 36 | q <<= 1; |
|---|
| 37 | r |= dividend.lo >> 31; |
|---|
| 38 | if (r >= divisor) |
|---|
| 39 | { |
|---|
| 40 | r -= divisor; |
|---|
| 41 | q |= 1; |
|---|
| 42 | } |
|---|
| 43 | dividend.lo <<= 1; |
|---|
| 44 | } while (i--); |
|---|
| 45 | return q; |
|---|
| 46 | } |
|---|
| 47 | |
|---|
| 48 | static UT_I32 MulDiv(UT_I32 multiplicand, UT_I32 multiplier, UT_I32 divisor) |
|---|
| 49 | { |
|---|
| 50 | UT_I32 s; |
|---|
| 51 | if (multiplier == divisor) return multiplicand; |
|---|
| 52 | s = multiplicand; multiplicand = (multiplicand < 0) ? -multiplicand : multiplicand; |
|---|
| 53 | s ^= multiplier; multiplier = (multiplier < 0) ? -multiplier : multiplier; |
|---|
| 54 | s ^= divisor; divisor = (divisor < 0) ? -divisor : divisor; |
|---|
| 55 | if ((multiplicand <= 46340) && (multiplier <= 46340) && (divisor <= 176095)) |
|---|
| 56 | { |
|---|
| 57 | multiplicand = (multiplicand * multiplier + (divisor >> 1)) / divisor; |
|---|
| 58 | return (s < 0) ? -multiplicand : multiplicand; |
|---|
| 59 | } |
|---|
| 60 | else |
|---|
| 61 | { |
|---|
| 62 | UT_I64 dividend; |
|---|
| 63 | Mul(multiplicand, multiplier, ÷nd); |
|---|
| 64 | return Div(dividend, divisor); |
|---|
| 65 | } |
|---|
| 66 | } |
|---|
| 67 | |
|---|
| 68 | static UT_FIXED MulFixed(UT_FIXED multiplicand, UT_FIXED multiplier) |
|---|
| 69 | { |
|---|
| 70 | UT_I32 s; |
|---|
| 71 | UT_U32 ua, ub; |
|---|
| 72 | if (multiplier == 0x10000) return multiplicand; |
|---|
| 73 | s = multiplicand; multiplicand = (multiplicand < 0) ? -multiplicand : multiplicand; |
|---|
| 74 | s ^= multiplier; multiplier = (multiplier < 0) ? -multiplier : multiplier; |
|---|
| 75 | ua = (UT_U32)multiplicand; |
|---|
| 76 | ub = (UT_U32)multiplier; |
|---|
| 77 | if ((ua <= 2048) && (ub <= 1048576)) |
|---|
| 78 | { |
|---|
| 79 | ua = (ua * ub + 0x8000) >> 16; |
|---|
| 80 | } |
|---|
| 81 | else |
|---|
| 82 | { |
|---|
| 83 | UT_U32 al = ua & 0xFFFF; |
|---|
| 84 | ua = (ua >> 16) * ub + al * (ub >> 16) + ((al * (ub & 0xFFFF) + 0x8000) >> 16); |
|---|
| 85 | } |
|---|
| 86 | return (s < 0) ? -(UT_FIXED)ua : (UT_FIXED)ua; |
|---|
| 87 | } |
|---|
| 88 | |
|---|
| 89 | static UT_FIXED DivFixed(UT_FIXED dividend, UT_FIXED divisor) |
|---|
| 90 | { |
|---|
| 91 | UT_I32 s; |
|---|
| 92 | UT_U32 q; |
|---|
| 93 | s = dividend; dividend = (dividend < 0) ? -dividend : dividend; |
|---|
| 94 | s ^= divisor; divisor = (divisor < 0) ? -divisor : divisor; |
|---|
| 95 | if (dividend >> 16) |
|---|
| 96 | { |
|---|
| 97 | UT_I64 dividend64; |
|---|
| 98 | dividend64.hi = dividend >> 16; |
|---|
| 99 | dividend64.lo = dividend << 16; |
|---|
| 100 | q = Div(dividend64, (UT_I32)divisor); |
|---|
| 101 | } |
|---|
| 102 | else |
|---|
| 103 | { |
|---|
| 104 | q = (UT_U32)((dividend << 16) + (divisor >> 1)) / (UT_U32)divisor; |
|---|
| 105 | } |
|---|
| 106 | return (s < 0) ? -(UT_FIXED)q : (UT_FIXED)q; |
|---|
| 107 | } |
|---|