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