1 /*
2 * Copyright 2012-15 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
26 #include "dm_services.h"
27 #include "include/fixed31_32.h"
28
29 static const struct fixed31_32 dc_fixpt_two_pi = { 26986075409LL };
30 static const struct fixed31_32 dc_fixpt_ln2 = { 2977044471LL };
31 static const struct fixed31_32 dc_fixpt_ln2_div_2 = { 1488522236LL };
32
abs_i64(long long arg)33 static inline unsigned long long abs_i64(
34 long long arg)
35 {
36 if (arg > 0)
37 return (unsigned long long)arg;
38 else
39 return (unsigned long long)(-arg);
40 }
41
42 /*
43 * @brief
44 * result = dividend / divisor
45 * *remainder = dividend % divisor
46 */
complete_integer_division_u64(unsigned long long dividend,unsigned long long divisor,unsigned long long * remainder)47 static inline unsigned long long complete_integer_division_u64(
48 unsigned long long dividend,
49 unsigned long long divisor,
50 unsigned long long *remainder)
51 {
52 unsigned long long result;
53
54 ASSERT(divisor);
55
56 result = div64_u64_rem(dividend, divisor, remainder);
57
58 return result;
59 }
60
61
62 #define FRACTIONAL_PART_MASK \
63 ((1ULL << FIXED31_32_BITS_PER_FRACTIONAL_PART) - 1)
64
65 #define GET_INTEGER_PART(x) \
66 ((x) >> FIXED31_32_BITS_PER_FRACTIONAL_PART)
67
68 #define GET_FRACTIONAL_PART(x) \
69 (FRACTIONAL_PART_MASK & (x))
70
dc_fixpt_from_fraction(long long numerator,long long denominator)71 struct fixed31_32 dc_fixpt_from_fraction(long long numerator, long long denominator)
72 {
73 struct fixed31_32 res;
74
75 bool arg1_negative = numerator < 0;
76 bool arg2_negative = denominator < 0;
77
78 unsigned long long arg1_value = arg1_negative ? -numerator : numerator;
79 unsigned long long arg2_value = arg2_negative ? -denominator : denominator;
80
81 unsigned long long remainder;
82
83 /* determine integer part */
84
85 unsigned long long res_value = complete_integer_division_u64(
86 arg1_value, arg2_value, &remainder);
87
88 ASSERT(res_value <= LONG_MAX);
89
90 /* determine fractional part */
91 {
92 unsigned int i = FIXED31_32_BITS_PER_FRACTIONAL_PART;
93
94 do {
95 remainder <<= 1;
96
97 res_value <<= 1;
98
99 if (remainder >= arg2_value) {
100 res_value |= 1;
101 remainder -= arg2_value;
102 }
103 } while (--i != 0);
104 }
105
106 /* round up LSB */
107 {
108 unsigned long long summand = (remainder << 1) >= arg2_value;
109
110 ASSERT(res_value <= LLONG_MAX - summand);
111
112 res_value += summand;
113 }
114
115 res.value = (long long)res_value;
116
117 if (arg1_negative ^ arg2_negative)
118 res.value = -res.value;
119
120 return res;
121 }
122
dc_fixpt_mul(struct fixed31_32 arg1,struct fixed31_32 arg2)123 struct fixed31_32 dc_fixpt_mul(struct fixed31_32 arg1, struct fixed31_32 arg2)
124 {
125 struct fixed31_32 res;
126
127 bool arg1_negative = arg1.value < 0;
128 bool arg2_negative = arg2.value < 0;
129
130 unsigned long long arg1_value = arg1_negative ? -arg1.value : arg1.value;
131 unsigned long long arg2_value = arg2_negative ? -arg2.value : arg2.value;
132
133 unsigned long long arg1_int = GET_INTEGER_PART(arg1_value);
134 unsigned long long arg2_int = GET_INTEGER_PART(arg2_value);
135
136 unsigned long long arg1_fra = GET_FRACTIONAL_PART(arg1_value);
137 unsigned long long arg2_fra = GET_FRACTIONAL_PART(arg2_value);
138
139 unsigned long long tmp;
140
141 res.value = arg1_int * arg2_int;
142
143 res.value <<= FIXED31_32_BITS_PER_FRACTIONAL_PART;
144
145 tmp = arg1_int * arg2_fra;
146
147 ASSERT(tmp <= (unsigned long long)(LLONG_MAX - res.value));
148
149 res.value += tmp;
150
151 tmp = arg2_int * arg1_fra;
152
153 ASSERT(tmp <= (unsigned long long)(LLONG_MAX - res.value));
154
155 res.value += tmp;
156
157 tmp = arg1_fra * arg2_fra;
158
159 tmp = (tmp >> FIXED31_32_BITS_PER_FRACTIONAL_PART) +
160 (tmp >= (unsigned long long)dc_fixpt_half.value);
161
162 ASSERT(tmp <= (unsigned long long)(LLONG_MAX - res.value));
163
164 res.value += tmp;
165
166 if (arg1_negative ^ arg2_negative)
167 res.value = -res.value;
168
169 return res;
170 }
171
dc_fixpt_sqr(struct fixed31_32 arg)172 struct fixed31_32 dc_fixpt_sqr(struct fixed31_32 arg)
173 {
174 struct fixed31_32 res;
175
176 unsigned long long arg_value = abs_i64(arg.value);
177
178 unsigned long long arg_int = GET_INTEGER_PART(arg_value);
179
180 unsigned long long arg_fra = GET_FRACTIONAL_PART(arg_value);
181
182 unsigned long long tmp;
183
184 res.value = arg_int * arg_int;
185
186 res.value <<= FIXED31_32_BITS_PER_FRACTIONAL_PART;
187
188 tmp = arg_int * arg_fra;
189
190 ASSERT(tmp <= (unsigned long long)(LLONG_MAX - res.value));
191
192 res.value += tmp;
193
194 ASSERT(tmp <= (unsigned long long)(LLONG_MAX - res.value));
195
196 res.value += tmp;
197
198 tmp = arg_fra * arg_fra;
199
200 tmp = (tmp >> FIXED31_32_BITS_PER_FRACTIONAL_PART) +
201 (tmp >= (unsigned long long)dc_fixpt_half.value);
202
203 ASSERT(tmp <= (unsigned long long)(LLONG_MAX - res.value));
204
205 res.value += tmp;
206
207 return res;
208 }
209
dc_fixpt_recip(struct fixed31_32 arg)210 struct fixed31_32 dc_fixpt_recip(struct fixed31_32 arg)
211 {
212 /*
213 * @note
214 * Good idea to use Newton's method
215 */
216
217 ASSERT(arg.value);
218
219 return dc_fixpt_from_fraction(
220 dc_fixpt_one.value,
221 arg.value);
222 }
223
dc_fixpt_sinc(struct fixed31_32 arg)224 struct fixed31_32 dc_fixpt_sinc(struct fixed31_32 arg)
225 {
226 struct fixed31_32 square;
227
228 struct fixed31_32 res = dc_fixpt_one;
229
230 int n = 27;
231
232 struct fixed31_32 arg_norm = arg;
233
234 if (dc_fixpt_le(
235 dc_fixpt_two_pi,
236 dc_fixpt_abs(arg))) {
237 arg_norm = dc_fixpt_sub(
238 arg_norm,
239 dc_fixpt_mul_int(
240 dc_fixpt_two_pi,
241 (int)div64_s64(
242 arg_norm.value,
243 dc_fixpt_two_pi.value)));
244 }
245
246 square = dc_fixpt_sqr(arg_norm);
247
248 do {
249 res = dc_fixpt_sub(
250 dc_fixpt_one,
251 dc_fixpt_div_int(
252 dc_fixpt_mul(
253 square,
254 res),
255 n * (n - 1)));
256
257 n -= 2;
258 } while (n > 2);
259
260 if (arg.value != arg_norm.value)
261 res = dc_fixpt_div(
262 dc_fixpt_mul(res, arg_norm),
263 arg);
264
265 return res;
266 }
267
dc_fixpt_sin(struct fixed31_32 arg)268 struct fixed31_32 dc_fixpt_sin(struct fixed31_32 arg)
269 {
270 return dc_fixpt_mul(
271 arg,
272 dc_fixpt_sinc(arg));
273 }
274
dc_fixpt_cos(struct fixed31_32 arg)275 struct fixed31_32 dc_fixpt_cos(struct fixed31_32 arg)
276 {
277 /* TODO implement argument normalization */
278
279 const struct fixed31_32 square = dc_fixpt_sqr(arg);
280
281 struct fixed31_32 res = dc_fixpt_one;
282
283 int n = 26;
284
285 do {
286 res = dc_fixpt_sub(
287 dc_fixpt_one,
288 dc_fixpt_div_int(
289 dc_fixpt_mul(
290 square,
291 res),
292 n * (n - 1)));
293
294 n -= 2;
295 } while (n != 0);
296
297 return res;
298 }
299
300 /*
301 * @brief
302 * result = exp(arg),
303 * where abs(arg) < 1
304 *
305 * Calculated as Taylor series.
306 */
fixed31_32_exp_from_taylor_series(struct fixed31_32 arg)307 static struct fixed31_32 fixed31_32_exp_from_taylor_series(struct fixed31_32 arg)
308 {
309 unsigned int n = 9;
310
311 struct fixed31_32 res = dc_fixpt_from_fraction(
312 n + 2,
313 n + 1);
314 /* TODO find correct res */
315
316 ASSERT(dc_fixpt_lt(arg, dc_fixpt_one));
317
318 do
319 res = dc_fixpt_add(
320 dc_fixpt_one,
321 dc_fixpt_div_int(
322 dc_fixpt_mul(
323 arg,
324 res),
325 n));
326 while (--n != 1);
327
328 return dc_fixpt_add(
329 dc_fixpt_one,
330 dc_fixpt_mul(
331 arg,
332 res));
333 }
334
dc_fixpt_exp(struct fixed31_32 arg)335 struct fixed31_32 dc_fixpt_exp(struct fixed31_32 arg)
336 {
337 /*
338 * @brief
339 * Main equation is:
340 * exp(x) = exp(r + m * ln(2)) = (1 << m) * exp(r),
341 * where m = round(x / ln(2)), r = x - m * ln(2)
342 */
343
344 if (dc_fixpt_le(
345 dc_fixpt_ln2_div_2,
346 dc_fixpt_abs(arg))) {
347 int m = dc_fixpt_round(
348 dc_fixpt_div(
349 arg,
350 dc_fixpt_ln2));
351
352 struct fixed31_32 r = dc_fixpt_sub(
353 arg,
354 dc_fixpt_mul_int(
355 dc_fixpt_ln2,
356 m));
357
358 ASSERT(m != 0);
359
360 ASSERT(dc_fixpt_lt(
361 dc_fixpt_abs(r),
362 dc_fixpt_one));
363
364 if (m > 0)
365 return dc_fixpt_shl(
366 fixed31_32_exp_from_taylor_series(r),
367 (unsigned char)m);
368 else
369 return dc_fixpt_div_int(
370 fixed31_32_exp_from_taylor_series(r),
371 1LL << -m);
372 } else if (arg.value != 0)
373 return fixed31_32_exp_from_taylor_series(arg);
374 else
375 return dc_fixpt_one;
376 }
377
dc_fixpt_log(struct fixed31_32 arg)378 struct fixed31_32 dc_fixpt_log(struct fixed31_32 arg)
379 {
380 struct fixed31_32 res = dc_fixpt_neg(dc_fixpt_one);
381 /* TODO improve 1st estimation */
382
383 struct fixed31_32 error;
384
385 ASSERT(arg.value > 0);
386 /* TODO if arg is negative, return NaN */
387 /* TODO if arg is zero, return -INF */
388
389 do {
390 struct fixed31_32 res1 = dc_fixpt_add(
391 dc_fixpt_sub(
392 res,
393 dc_fixpt_one),
394 dc_fixpt_div(
395 arg,
396 dc_fixpt_exp(res)));
397
398 error = dc_fixpt_sub(
399 res,
400 res1);
401
402 res = res1;
403 /* TODO determine max_allowed_error based on quality of exp() */
404 } while (abs_i64(error.value) > 100ULL);
405
406 return res;
407 }
408
409
410 /* this function is a generic helper to translate fixed point value to
411 * specified integer format that will consist of integer_bits integer part and
412 * fractional_bits fractional part. For example it is used in
413 * dc_fixpt_u2d19 to receive 2 bits integer part and 19 bits fractional
414 * part in 32 bits. It is used in hw programming (scaler)
415 */
416
ux_dy(long long value,unsigned int integer_bits,unsigned int fractional_bits)417 static inline unsigned int ux_dy(
418 long long value,
419 unsigned int integer_bits,
420 unsigned int fractional_bits)
421 {
422 /* 1. create mask of integer part */
423 unsigned int result = (1 << integer_bits) - 1;
424 /* 2. mask out fractional part */
425 unsigned int fractional_part = FRACTIONAL_PART_MASK & value;
426 /* 3. shrink fixed point integer part to be of integer_bits width*/
427 result &= GET_INTEGER_PART(value);
428 /* 4. make space for fractional part to be filled in after integer */
429 result <<= fractional_bits;
430 /* 5. shrink fixed point fractional part to of fractional_bits width*/
431 fractional_part >>= FIXED31_32_BITS_PER_FRACTIONAL_PART - fractional_bits;
432 /* 6. merge the result */
433 return result | fractional_part;
434 }
435
clamp_ux_dy(long long value,unsigned int integer_bits,unsigned int fractional_bits,unsigned int min_clamp)436 static inline unsigned int clamp_ux_dy(
437 long long value,
438 unsigned int integer_bits,
439 unsigned int fractional_bits,
440 unsigned int min_clamp)
441 {
442 unsigned int truncated_val = ux_dy(value, integer_bits, fractional_bits);
443
444 if (value >= (1LL << (integer_bits + FIXED31_32_BITS_PER_FRACTIONAL_PART)))
445 return (1 << (integer_bits + fractional_bits)) - 1;
446 else if (truncated_val > min_clamp)
447 return truncated_val;
448 else
449 return min_clamp;
450 }
451
dc_fixpt_u4d19(struct fixed31_32 arg)452 unsigned int dc_fixpt_u4d19(struct fixed31_32 arg)
453 {
454 return ux_dy(arg.value, 4, 19);
455 }
456
dc_fixpt_u3d19(struct fixed31_32 arg)457 unsigned int dc_fixpt_u3d19(struct fixed31_32 arg)
458 {
459 return ux_dy(arg.value, 3, 19);
460 }
461
dc_fixpt_u2d19(struct fixed31_32 arg)462 unsigned int dc_fixpt_u2d19(struct fixed31_32 arg)
463 {
464 return ux_dy(arg.value, 2, 19);
465 }
466
dc_fixpt_u0d19(struct fixed31_32 arg)467 unsigned int dc_fixpt_u0d19(struct fixed31_32 arg)
468 {
469 return ux_dy(arg.value, 0, 19);
470 }
471
dc_fixpt_clamp_u0d14(struct fixed31_32 arg)472 unsigned int dc_fixpt_clamp_u0d14(struct fixed31_32 arg)
473 {
474 return clamp_ux_dy(arg.value, 0, 14, 1);
475 }
476
dc_fixpt_clamp_u0d10(struct fixed31_32 arg)477 unsigned int dc_fixpt_clamp_u0d10(struct fixed31_32 arg)
478 {
479 return clamp_ux_dy(arg.value, 0, 10, 1);
480 }
481
dc_fixpt_s4d19(struct fixed31_32 arg)482 int dc_fixpt_s4d19(struct fixed31_32 arg)
483 {
484 if (arg.value < 0)
485 return -(int)ux_dy(dc_fixpt_abs(arg).value, 4, 19);
486 else
487 return ux_dy(arg.value, 4, 19);
488 }
489
dc_fixpt_from_ux_dy(unsigned int value,unsigned int integer_bits,unsigned int fractional_bits)490 struct fixed31_32 dc_fixpt_from_ux_dy(unsigned int value,
491 unsigned int integer_bits,
492 unsigned int fractional_bits)
493 {
494 struct fixed31_32 fixpt_value = dc_fixpt_zero;
495 struct fixed31_32 fixpt_int_value = dc_fixpt_zero;
496 long long frac_mask = ((long long)1 << (long long)integer_bits) - 1;
497
498 fixpt_value.value = (long long)value << (FIXED31_32_BITS_PER_FRACTIONAL_PART - fractional_bits);
499 frac_mask = frac_mask << fractional_bits;
500 fixpt_int_value.value = value & frac_mask;
501 fixpt_int_value.value <<= (FIXED31_32_BITS_PER_FRACTIONAL_PART - fractional_bits);
502 fixpt_value.value |= fixpt_int_value.value;
503 return fixpt_value;
504 }
505
dc_fixpt_from_int_dy(unsigned int int_value,unsigned int frac_value,unsigned int integer_bits,unsigned int fractional_bits)506 struct fixed31_32 dc_fixpt_from_int_dy(unsigned int int_value,
507 unsigned int frac_value,
508 unsigned int integer_bits,
509 unsigned int fractional_bits)
510 {
511 struct fixed31_32 fixpt_value = dc_fixpt_from_int(int_value);
512
513 fixpt_value.value |= (long long)frac_value << (FIXED31_32_BITS_PER_FRACTIONAL_PART - fractional_bits);
514 return fixpt_value;
515 }
516