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