1 /*
2 * Copyright (c) 2013, Google Inc. All rights reserved.
3 *
4 * Permission is hereby granted, free of charge, to any person obtaining
5 * a copy of this software and associated documentation files
6 * (the "Software"), to deal in the Software without restriction,
7 * including without limitation the rights to use, copy, modify, merge,
8 * publish, distribute, sublicense, and/or sell copies of the Software,
9 * and to permit persons to whom the Software is furnished to do so,
10 * subject to the following conditions:
11 *
12 * The above copyright notice and this permission notice shall be
13 * included in all copies or substantial portions of the Software.
14 *
15 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
16 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
17 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
18 * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
19 * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
20 * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
21 * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
22 */
23
24 #pragma once
25
26 #include <stdint.h>
27
28 #ifndef DEBUG_FIXED_POINT
29 #define DEBUG_FIXED_POINT 0
30 #endif
31
32 struct fp_32_64 {
33 uint32_t l0; /* unshifted value */
34 uint32_t l32; /* value shifted left 32 bits (or bit -1 to -32) */
35 uint32_t l64; /* value shifted left 64 bits (or bit -33 to -64) */
36 };
37
38 #include "fixed_point_debug.h"
39
40 static void
fp_32_64_div_32_32(struct fp_32_64 * result,uint32_t dividend,uint32_t divisor)41 fp_32_64_div_32_32(struct fp_32_64 *result, uint32_t dividend, uint32_t divisor)
42 {
43 uint64_t tmp;
44 uint32_t rem;
45
46 tmp = ((uint64_t)dividend << 32) / divisor;
47 rem = ((uint64_t)dividend << 32) % divisor;
48 result->l0 = tmp >> 32;
49 result->l32 = (uint32_t)tmp;
50 tmp = ((uint64_t)rem << 32) / divisor;
51 result->l64 = tmp;
52 }
53
54 static uint64_t
mul_u32_u32(uint32_t a,uint32_t b,int a_shift,int b_shift)55 mul_u32_u32(uint32_t a, uint32_t b, int a_shift, int b_shift)
56 {
57 uint64_t ret = (uint64_t)a * b;
58 debug_mul_u32_u32(a, b, a_shift, b_shift, ret);
59 return ret;
60 }
61
62 static uint64_t
u64_mul_u32_fp32_64(uint32_t a,struct fp_32_64 b)63 u64_mul_u32_fp32_64(uint32_t a, struct fp_32_64 b)
64 {
65 uint64_t tmp;
66 uint64_t res_0;
67 uint64_t res_l32;
68 uint32_t res_l32_32;
69 uint64_t ret;
70
71 res_0 = mul_u32_u32(a, b.l0, 0, 0);
72 tmp = mul_u32_u32(a, b.l32, 0, -32);
73 res_0 += tmp >> 32;
74 res_l32 = (uint32_t)tmp;
75 res_l32 += mul_u32_u32(a, b.l64, 0, -64) >> 32; /* Improve rounding accuracy */
76 res_0 += res_l32 >> 32;
77 res_l32_32 = res_l32;
78 ret = res_0 + (res_l32_32 >> 31); /* Round to nearest integer */
79
80 debug_u64_mul_u32_fp32_64(a, b, res_0, res_l32_32, ret);
81
82 return ret;
83 }
84
85 static uint32_t
u32_mul_u64_fp32_64(uint64_t a,struct fp_32_64 b)86 u32_mul_u64_fp32_64(uint64_t a, struct fp_32_64 b)
87 {
88 uint32_t a_r32 = a >> 32;
89 uint32_t a_0 = (uint32_t)a;
90 uint64_t res_l32;
91 uint32_t ret;
92
93 /* mul_u32_u32(a_r32, b.l0, 32, 0) does not affect result */
94 res_l32 = mul_u32_u32(a_0, b.l0, 0, 0) << 32;
95 res_l32 += mul_u32_u32(a_r32, b.l32, 32, -32) << 32;
96 res_l32 += mul_u32_u32(a_0, b.l32, 0, -32);
97 res_l32 += mul_u32_u32(a_r32, b.l64, 32, -64);
98 res_l32 += mul_u32_u32(a_0, b.l64, 0, -64) >> 32; /* Improve rounding accuracy */
99 ret = (uint32_t)((res_l32 >> 32) + ((uint32_t)res_l32 >> 31)); /* Round to nearest integer */
100
101 debug_u32_mul_u64_fp32_64(a, b, res_l32, ret);
102
103 return ret;
104 }
105
106 static uint64_t
u64_mul_u64_fp32_64(uint64_t a,struct fp_32_64 b)107 u64_mul_u64_fp32_64(uint64_t a, struct fp_32_64 b)
108 {
109 uint32_t a_r32 = a >> 32;
110 uint32_t a_0 = (uint32_t)a;
111 uint64_t res_0;
112 uint64_t res_l32;
113 uint32_t res_l32_32;
114 uint64_t tmp;
115 uint64_t ret;
116
117 tmp = mul_u32_u32(a_r32, b.l0, 32, 0);
118 res_0 = tmp << 32;
119 tmp = mul_u32_u32(a_0, b.l0, 0, 0);
120 res_0 += tmp;
121 tmp = mul_u32_u32(a_r32, b.l32, 32, -32);
122 res_0 += tmp;
123 tmp = mul_u32_u32(a_0, b.l32, 0, -32);
124 res_0 += tmp >> 32;
125 res_l32 = (uint32_t)tmp;
126 tmp = mul_u32_u32(a_r32, b.l64, 32, -64);
127 res_0 += tmp >> 32;
128 res_l32 += (uint32_t)tmp;
129 tmp = mul_u32_u32(a_0, b.l64, 0, -64); /* Improve rounding accuracy */
130 res_l32 += tmp >> 32;
131 res_0 += res_l32 >> 32;
132 res_l32_32 = (uint32_t)res_l32;
133 ret = res_0 + (res_l32_32 >> 31); /* Round to nearest integer */
134
135 debug_u64_mul_u64_fp32_64(a, b, res_0, res_l32_32, ret);
136
137 return ret;
138 }
139
140