1 /*
2  * Copyright 2015 Advanced Micro Devices, Inc.
3  *
4  * Permission is hereby granted, free of charge, to any person obtaining a
5  * copy of this software and associated documentation files (the "Software"),
6  * to deal in the Software without restriction, including without limitation
7  * the rights to use, copy, modify, merge, publish, distribute, sublicense,
8  * and/or sell copies of the Software, and to permit persons to whom the
9  * Software is furnished to do so, subject to the following conditions:
10  *
11  * The above copyright notice and this permission notice shall be included in
12  * all copies or substantial portions of the Software.
13  *
14  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
15  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
16  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
17  * THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
18  * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
19  * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
20  * OTHER DEALINGS IN THE SOFTWARE.
21  *
22  * Authors: AMD
23  *
24  */
25 #include "dm_services.h"
26 #include "bw_fixed.h"
27 
28 #define MAX_I64 \
29 	((int64_t)((1ULL << 63) - 1))
30 
31 #define MIN_I64 \
32 	(-MAX_I64 - 1)
33 
34 #define FRACTIONAL_PART_MASK \
35 	((1ULL << BW_FIXED_BITS_PER_FRACTIONAL_PART) - 1)
36 
37 #define GET_FRACTIONAL_PART(x) \
38 	(FRACTIONAL_PART_MASK & (x))
39 
abs_i64(int64_t arg)40 static uint64_t abs_i64(int64_t arg)
41 {
42 	if (arg >= 0)
43 		return (uint64_t)(arg);
44 	else
45 		return (uint64_t)(-arg);
46 }
47 
bw_int_to_fixed_nonconst(int64_t value)48 struct bw_fixed bw_int_to_fixed_nonconst(int64_t value)
49 {
50 	struct bw_fixed res;
51 
52 	ASSERT(value < BW_FIXED_MAX_I32 && value > BW_FIXED_MIN_I32);
53 	res.value = value << BW_FIXED_BITS_PER_FRACTIONAL_PART;
54 	return res;
55 }
56 
bw_frc_to_fixed(int64_t numerator,int64_t denominator)57 struct bw_fixed bw_frc_to_fixed(int64_t numerator, int64_t denominator)
58 {
59 	struct bw_fixed res;
60 	bool arg1_negative = numerator < 0;
61 	bool arg2_negative = denominator < 0;
62 	uint64_t arg1_value;
63 	uint64_t arg2_value;
64 	uint64_t remainder;
65 
66 	/* determine integer part */
67 	uint64_t res_value;
68 
69 	ASSERT(denominator != 0);
70 
71 	arg1_value = abs_i64(numerator);
72 	arg2_value = abs_i64(denominator);
73 	res_value = div64_u64_rem(arg1_value, arg2_value, &remainder);
74 
75 	ASSERT(res_value <= BW_FIXED_MAX_I32);
76 
77 	/* determine fractional part */
78 	{
79 		uint32_t i = BW_FIXED_BITS_PER_FRACTIONAL_PART;
80 
81 		do {
82 			remainder <<= 1;
83 
84 			res_value <<= 1;
85 
86 			if (remainder >= arg2_value) {
87 				res_value |= 1;
88 				remainder -= arg2_value;
89 			}
90 		} while (--i != 0);
91 	}
92 
93 	/* round up LSB */
94 	{
95 		uint64_t summand = (remainder << 1) >= arg2_value;
96 
97 		ASSERT(res_value <= MAX_I64 - summand);
98 
99 		res_value += summand;
100 	}
101 
102 	res.value = (int64_t)(res_value);
103 
104 	if (arg1_negative ^ arg2_negative)
105 		res.value = -res.value;
106 	return res;
107 }
108 
bw_floor2(const struct bw_fixed arg,const struct bw_fixed significance)109 struct bw_fixed bw_floor2(
110 	const struct bw_fixed arg,
111 	const struct bw_fixed significance)
112 {
113 	struct bw_fixed result;
114 	int64_t multiplicand;
115 
116 	multiplicand = div64_s64(arg.value, abs_i64(significance.value));
117 	result.value = abs_i64(significance.value) * multiplicand;
118 	ASSERT(abs_i64(result.value) <= abs_i64(arg.value));
119 	return result;
120 }
121 
bw_ceil2(const struct bw_fixed arg,const struct bw_fixed significance)122 struct bw_fixed bw_ceil2(
123 	const struct bw_fixed arg,
124 	const struct bw_fixed significance)
125 {
126 	struct bw_fixed result;
127 	int64_t multiplicand;
128 
129 	multiplicand = div64_s64(arg.value, abs_i64(significance.value));
130 	result.value = abs_i64(significance.value) * multiplicand;
131 	if (abs_i64(result.value) < abs_i64(arg.value)) {
132 		if (arg.value < 0)
133 			result.value -= abs_i64(significance.value);
134 		else
135 			result.value += abs_i64(significance.value);
136 	}
137 	return result;
138 }
139 
bw_mul(const struct bw_fixed arg1,const struct bw_fixed arg2)140 struct bw_fixed bw_mul(const struct bw_fixed arg1, const struct bw_fixed arg2)
141 {
142 	struct bw_fixed res;
143 
144 	bool arg1_negative = arg1.value < 0;
145 	bool arg2_negative = arg2.value < 0;
146 
147 	uint64_t arg1_value = abs_i64(arg1.value);
148 	uint64_t arg2_value = abs_i64(arg2.value);
149 
150 	uint64_t arg1_int = BW_FIXED_GET_INTEGER_PART(arg1_value);
151 	uint64_t arg2_int = BW_FIXED_GET_INTEGER_PART(arg2_value);
152 
153 	uint64_t arg1_fra = GET_FRACTIONAL_PART(arg1_value);
154 	uint64_t arg2_fra = GET_FRACTIONAL_PART(arg2_value);
155 
156 	uint64_t tmp;
157 
158 	res.value = arg1_int * arg2_int;
159 
160 	ASSERT(res.value <= BW_FIXED_MAX_I32);
161 
162 	res.value <<= BW_FIXED_BITS_PER_FRACTIONAL_PART;
163 
164 	tmp = arg1_int * arg2_fra;
165 
166 	ASSERT(tmp <= (uint64_t)(MAX_I64 - res.value));
167 
168 	res.value += tmp;
169 
170 	tmp = arg2_int * arg1_fra;
171 
172 	ASSERT(tmp <= (uint64_t)(MAX_I64 - res.value));
173 
174 	res.value += tmp;
175 
176 	tmp = arg1_fra * arg2_fra;
177 
178 	tmp = (tmp >> BW_FIXED_BITS_PER_FRACTIONAL_PART) +
179 		(tmp >= (uint64_t)(bw_frc_to_fixed(1, 2).value));
180 
181 	ASSERT(tmp <= (uint64_t)(MAX_I64 - res.value));
182 
183 	res.value += tmp;
184 
185 	if (arg1_negative ^ arg2_negative)
186 		res.value = -res.value;
187 	return res;
188 }
189 
190