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
 29static inline uint64_t abs_i64(
 30	int64_t arg)
 31{
 32	if (arg > 0)
 33		return (uint64_t)arg;
 34	else
 35		return (uint64_t)(-arg);
 36}
 37
 38/*
 39 * @brief
 40 * result = dividend / divisor
 41 * *remainder = dividend % divisor
 42 */
 43static inline uint64_t complete_integer_division_u64(
 44	uint64_t dividend,
 45	uint64_t divisor,
 46	uint64_t *remainder)
 47{
 48	uint64_t result;
 49
 50	ASSERT(divisor);
 51
 52	result = div64_u64_rem(dividend, divisor, remainder);
 53
 54	return result;
 55}
 56
 57
 58#define FRACTIONAL_PART_MASK \
 59	((1ULL << FIXED31_32_BITS_PER_FRACTIONAL_PART) - 1)
 60
 61#define GET_INTEGER_PART(x) \
 62	((x) >> FIXED31_32_BITS_PER_FRACTIONAL_PART)
 63
 64#define GET_FRACTIONAL_PART(x) \
 65	(FRACTIONAL_PART_MASK & (x))
 66
 67struct fixed31_32 dal_fixed31_32_from_fraction(
 68	int64_t numerator,
 69	int64_t denominator)
 70{
 71	struct fixed31_32 res;
 72
 73	bool arg1_negative = numerator < 0;
 74	bool arg2_negative = denominator < 0;
 75
 76	uint64_t arg1_value = arg1_negative ? -numerator : numerator;
 77	uint64_t arg2_value = arg2_negative ? -denominator : denominator;
 78
 79	uint64_t remainder;
 80
 81	/* determine integer part */
 82
 83	uint64_t res_value = complete_integer_division_u64(
 84		arg1_value, arg2_value, &remainder);
 85
 86	ASSERT(res_value <= LONG_MAX);
 87
 88	/* determine fractional part */
 89	{
 90		uint32_t i = FIXED31_32_BITS_PER_FRACTIONAL_PART;
 91
 92		do {
 93			remainder <<= 1;
 94
 95			res_value <<= 1;
 96
 97			if (remainder >= arg2_value) {
 98				res_value |= 1;
 99				remainder -= arg2_value;
100			}
101		} while (--i != 0);
102	}
103
104	/* round up LSB */
105	{
106		uint64_t summand = (remainder << 1) >= arg2_value;
107
108		ASSERT(res_value <= LLONG_MAX - summand);
109
110		res_value += summand;
111	}
112
113	res.value = (int64_t)res_value;
114
115	if (arg1_negative ^ arg2_negative)
116		res.value = -res.value;
117
118	return res;
119}
120
121struct fixed31_32 dal_fixed31_32_from_int_nonconst(
122	int64_t arg)
123{
124	struct fixed31_32 res;
125
126	ASSERT((LONG_MIN <= arg) && (arg <= LONG_MAX));
127
128	res.value = arg << FIXED31_32_BITS_PER_FRACTIONAL_PART;
129
130	return res;
131}
132
133struct fixed31_32 dal_fixed31_32_shl(
134	struct fixed31_32 arg,
135	uint8_t shift)
136{
137	struct fixed31_32 res;
138
139	ASSERT(((arg.value >= 0) && (arg.value <= LLONG_MAX >> shift)) ||
140		((arg.value < 0) && (arg.value >= LLONG_MIN >> shift)));
141
142	res.value = arg.value << shift;
143
144	return res;
145}
146
147struct fixed31_32 dal_fixed31_32_add(
148	struct fixed31_32 arg1,
149	struct fixed31_32 arg2)
150{
151	struct fixed31_32 res;
152
153	ASSERT(((arg1.value >= 0) && (LLONG_MAX - arg1.value >= arg2.value)) ||
154		((arg1.value < 0) && (LLONG_MIN - arg1.value <= arg2.value)));
155
156	res.value = arg1.value + arg2.value;
157
158	return res;
159}
160
161struct fixed31_32 dal_fixed31_32_sub(
162	struct fixed31_32 arg1,
163	struct fixed31_32 arg2)
164{
165	struct fixed31_32 res;
166
167	ASSERT(((arg2.value >= 0) && (LLONG_MIN + arg2.value <= arg1.value)) ||
168		((arg2.value < 0) && (LLONG_MAX + arg2.value >= arg1.value)));
169
170	res.value = arg1.value - arg2.value;
171
172	return res;
173}
174
175struct fixed31_32 dal_fixed31_32_mul(
176	struct fixed31_32 arg1,
177	struct fixed31_32 arg2)
178{
179	struct fixed31_32 res;
180
181	bool arg1_negative = arg1.value < 0;
182	bool arg2_negative = arg2.value < 0;
183
184	uint64_t arg1_value = arg1_negative ? -arg1.value : arg1.value;
185	uint64_t arg2_value = arg2_negative ? -arg2.value : arg2.value;
186
187	uint64_t arg1_int = GET_INTEGER_PART(arg1_value);
188	uint64_t arg2_int = GET_INTEGER_PART(arg2_value);
189
190	uint64_t arg1_fra = GET_FRACTIONAL_PART(arg1_value);
191	uint64_t arg2_fra = GET_FRACTIONAL_PART(arg2_value);
192
193	uint64_t tmp;
194
195	res.value = arg1_int * arg2_int;
196
197	ASSERT(res.value <= LONG_MAX);
198
199	res.value <<= FIXED31_32_BITS_PER_FRACTIONAL_PART;
200
201	tmp = arg1_int * arg2_fra;
202
203	ASSERT(tmp <= (uint64_t)(LLONG_MAX - res.value));
204
205	res.value += tmp;
206
207	tmp = arg2_int * arg1_fra;
208
209	ASSERT(tmp <= (uint64_t)(LLONG_MAX - res.value));
210
211	res.value += tmp;
212
213	tmp = arg1_fra * arg2_fra;
214
215	tmp = (tmp >> FIXED31_32_BITS_PER_FRACTIONAL_PART) +
216		(tmp >= (uint64_t)dal_fixed31_32_half.value);
217
218	ASSERT(tmp <= (uint64_t)(LLONG_MAX - res.value));
219
220	res.value += tmp;
221
222	if (arg1_negative ^ arg2_negative)
223		res.value = -res.value;
224
225	return res;
226}
227
228struct fixed31_32 dal_fixed31_32_sqr(
229	struct fixed31_32 arg)
230{
231	struct fixed31_32 res;
232
233	uint64_t arg_value = abs_i64(arg.value);
234
235	uint64_t arg_int = GET_INTEGER_PART(arg_value);
236
237	uint64_t arg_fra = GET_FRACTIONAL_PART(arg_value);
238
239	uint64_t tmp;
240
241	res.value = arg_int * arg_int;
242
243	ASSERT(res.value <= LONG_MAX);
244
245	res.value <<= FIXED31_32_BITS_PER_FRACTIONAL_PART;
246
247	tmp = arg_int * arg_fra;
248
249	ASSERT(tmp <= (uint64_t)(LLONG_MAX - res.value));
250
251	res.value += tmp;
252
253	ASSERT(tmp <= (uint64_t)(LLONG_MAX - res.value));
254
255	res.value += tmp;
256
257	tmp = arg_fra * arg_fra;
258
259	tmp = (tmp >> FIXED31_32_BITS_PER_FRACTIONAL_PART) +
260		(tmp >= (uint64_t)dal_fixed31_32_half.value);
261
262	ASSERT(tmp <= (uint64_t)(LLONG_MAX - res.value));
263
264	res.value += tmp;
265
266	return res;
267}
268
269struct fixed31_32 dal_fixed31_32_recip(
270	struct fixed31_32 arg)
271{
272	/*
273	 * @note
274	 * Good idea to use Newton's method
275	 */
276
277	ASSERT(arg.value);
278
279	return dal_fixed31_32_from_fraction(
280		dal_fixed31_32_one.value,
281		arg.value);
282}
283
284struct fixed31_32 dal_fixed31_32_sinc(
285	struct fixed31_32 arg)
286{
287	struct fixed31_32 square;
288
289	struct fixed31_32 res = dal_fixed31_32_one;
290
291	int32_t n = 27;
292
293	struct fixed31_32 arg_norm = arg;
294
295	if (dal_fixed31_32_le(
296		dal_fixed31_32_two_pi,
297		dal_fixed31_32_abs(arg))) {
298		arg_norm = dal_fixed31_32_sub(
299			arg_norm,
300			dal_fixed31_32_mul_int(
301				dal_fixed31_32_two_pi,
302				(int32_t)div64_s64(
303					arg_norm.value,
304					dal_fixed31_32_two_pi.value)));
305	}
306
307	square = dal_fixed31_32_sqr(arg_norm);
308
309	do {
310		res = dal_fixed31_32_sub(
311			dal_fixed31_32_one,
312			dal_fixed31_32_div_int(
313				dal_fixed31_32_mul(
314					square,
315					res),
316				n * (n - 1)));
317
318		n -= 2;
319	} while (n > 2);
320
321	if (arg.value != arg_norm.value)
322		res = dal_fixed31_32_div(
323			dal_fixed31_32_mul(res, arg_norm),
324			arg);
325
326	return res;
327}
328
329struct fixed31_32 dal_fixed31_32_sin(
330	struct fixed31_32 arg)
331{
332	return dal_fixed31_32_mul(
333		arg,
334		dal_fixed31_32_sinc(arg));
335}
336
337struct fixed31_32 dal_fixed31_32_cos(
338	struct fixed31_32 arg)
339{
340	/* TODO implement argument normalization */
341
342	const struct fixed31_32 square = dal_fixed31_32_sqr(arg);
343
344	struct fixed31_32 res = dal_fixed31_32_one;
345
346	int32_t n = 26;
347
348	do {
349		res = dal_fixed31_32_sub(
350			dal_fixed31_32_one,
351			dal_fixed31_32_div_int(
352				dal_fixed31_32_mul(
353					square,
354					res),
355				n * (n - 1)));
356
357		n -= 2;
358	} while (n != 0);
359
360	return res;
361}
362
363/*
364 * @brief
365 * result = exp(arg),
366 * where abs(arg) < 1
367 *
368 * Calculated as Taylor series.
369 */
370static struct fixed31_32 fixed31_32_exp_from_taylor_series(
371	struct fixed31_32 arg)
372{
373	uint32_t n = 9;
374
375	struct fixed31_32 res = dal_fixed31_32_from_fraction(
376		n + 2,
377		n + 1);
378	/* TODO find correct res */
379
380	ASSERT(dal_fixed31_32_lt(arg, dal_fixed31_32_one));
381
382	do
383		res = dal_fixed31_32_add(
384			dal_fixed31_32_one,
385			dal_fixed31_32_div_int(
386				dal_fixed31_32_mul(
387					arg,
388					res),
389				n));
390	while (--n != 1);
391
392	return dal_fixed31_32_add(
393		dal_fixed31_32_one,
394		dal_fixed31_32_mul(
395			arg,
396			res));
397}
398
399struct fixed31_32 dal_fixed31_32_exp(
400	struct fixed31_32 arg)
401{
402	/*
403	 * @brief
404	 * Main equation is:
405	 * exp(x) = exp(r + m * ln(2)) = (1 << m) * exp(r),
406	 * where m = round(x / ln(2)), r = x - m * ln(2)
407	 */
408
409	if (dal_fixed31_32_le(
410		dal_fixed31_32_ln2_div_2,
411		dal_fixed31_32_abs(arg))) {
412		int32_t m = dal_fixed31_32_round(
413			dal_fixed31_32_div(
414				arg,
415				dal_fixed31_32_ln2));
416
417		struct fixed31_32 r = dal_fixed31_32_sub(
418			arg,
419			dal_fixed31_32_mul_int(
420				dal_fixed31_32_ln2,
421				m));
422
423		ASSERT(m != 0);
424
425		ASSERT(dal_fixed31_32_lt(
426			dal_fixed31_32_abs(r),
427			dal_fixed31_32_one));
428
429		if (m > 0)
430			return dal_fixed31_32_shl(
431				fixed31_32_exp_from_taylor_series(r),
432				(uint8_t)m);
433		else
434			return dal_fixed31_32_div_int(
435				fixed31_32_exp_from_taylor_series(r),
436				1LL << -m);
437	} else if (arg.value != 0)
438		return fixed31_32_exp_from_taylor_series(arg);
439	else
440		return dal_fixed31_32_one;
441}
442
443struct fixed31_32 dal_fixed31_32_log(
444	struct fixed31_32 arg)
445{
446	struct fixed31_32 res = dal_fixed31_32_neg(dal_fixed31_32_one);
447	/* TODO improve 1st estimation */
448
449	struct fixed31_32 error;
450
451	ASSERT(arg.value > 0);
452	/* TODO if arg is negative, return NaN */
453	/* TODO if arg is zero, return -INF */
454
455	do {
456		struct fixed31_32 res1 = dal_fixed31_32_add(
457			dal_fixed31_32_sub(
458				res,
459				dal_fixed31_32_one),
460			dal_fixed31_32_div(
461				arg,
462				dal_fixed31_32_exp(res)));
463
464		error = dal_fixed31_32_sub(
465			res,
466			res1);
467
468		res = res1;
469		/* TODO determine max_allowed_error based on quality of exp() */
470	} while (abs_i64(error.value) > 100ULL);
471
472	return res;
473}
474
475struct fixed31_32 dal_fixed31_32_pow(
476	struct fixed31_32 arg1,
477	struct fixed31_32 arg2)
478{
479	return dal_fixed31_32_exp(
480		dal_fixed31_32_mul(
481			dal_fixed31_32_log(arg1),
482			arg2));
483}
484
485int32_t dal_fixed31_32_floor(
486	struct fixed31_32 arg)
487{
488	uint64_t arg_value = abs_i64(arg.value);
489
490	if (arg.value >= 0)
491		return (int32_t)GET_INTEGER_PART(arg_value);
492	else
493		return -(int32_t)GET_INTEGER_PART(arg_value);
494}
495
496int32_t dal_fixed31_32_round(
497	struct fixed31_32 arg)
498{
499	uint64_t arg_value = abs_i64(arg.value);
500
501	const int64_t summand = dal_fixed31_32_half.value;
502
503	ASSERT(LLONG_MAX - (int64_t)arg_value >= summand);
504
505	arg_value += summand;
506
507	if (arg.value >= 0)
508		return (int32_t)GET_INTEGER_PART(arg_value);
509	else
510		return -(int32_t)GET_INTEGER_PART(arg_value);
511}
512
513int32_t dal_fixed31_32_ceil(
514	struct fixed31_32 arg)
515{
516	uint64_t arg_value = abs_i64(arg.value);
517
518	const int64_t summand = dal_fixed31_32_one.value -
519		dal_fixed31_32_epsilon.value;
520
521	ASSERT(LLONG_MAX - (int64_t)arg_value >= summand);
522
523	arg_value += summand;
524
525	if (arg.value >= 0)
526		return (int32_t)GET_INTEGER_PART(arg_value);
527	else
528		return -(int32_t)GET_INTEGER_PART(arg_value);
529}
530
531/* this function is a generic helper to translate fixed point value to
532 * specified integer format that will consist of integer_bits integer part and
533 * fractional_bits fractional part. For example it is used in
534 * dal_fixed31_32_u2d19 to receive 2 bits integer part and 19 bits fractional
535 * part in 32 bits. It is used in hw programming (scaler)
536 */
537
538static inline uint32_t ux_dy(
539	int64_t value,
540	uint32_t integer_bits,
541	uint32_t fractional_bits)
542{
543	/* 1. create mask of integer part */
544	uint32_t result = (1 << integer_bits) - 1;
545	/* 2. mask out fractional part */
546	uint32_t fractional_part = FRACTIONAL_PART_MASK & value;
547	/* 3. shrink fixed point integer part to be of integer_bits width*/
548	result &= GET_INTEGER_PART(value);
549	/* 4. make space for fractional part to be filled in after integer */
550	result <<= fractional_bits;
551	/* 5. shrink fixed point fractional part to of fractional_bits width*/
552	fractional_part >>= FIXED31_32_BITS_PER_FRACTIONAL_PART - fractional_bits;
553	/* 6. merge the result */
554	return result | fractional_part;
555}
556
557static inline uint32_t clamp_ux_dy(
558	int64_t value,
559	uint32_t integer_bits,
560	uint32_t fractional_bits,
561	uint32_t min_clamp)
562{
563	uint32_t truncated_val = ux_dy(value, integer_bits, fractional_bits);
564
565	if (value >= (1LL << (integer_bits + FIXED31_32_BITS_PER_FRACTIONAL_PART)))
566		return (1 << (integer_bits + fractional_bits)) - 1;
567	else if (truncated_val > min_clamp)
568		return truncated_val;
569	else
570		return min_clamp;
571}
572
573uint32_t dal_fixed31_32_u2d19(
574	struct fixed31_32 arg)
575{
576	return ux_dy(arg.value, 2, 19);
577}
578
579uint32_t dal_fixed31_32_u0d19(
580	struct fixed31_32 arg)
581{
582	return ux_dy(arg.value, 0, 19);
583}
584
585uint32_t dal_fixed31_32_clamp_u0d14(
586	struct fixed31_32 arg)
587{
588	return clamp_ux_dy(arg.value, 0, 14, 1);
589}
590
591uint32_t dal_fixed31_32_clamp_u0d10(
592	struct fixed31_32 arg)
593{
594	return clamp_ux_dy(arg.value, 0, 10, 1);
595}
596
597int32_t dal_fixed31_32_s4d19(
598	struct fixed31_32 arg)
599{
600	if (arg.value < 0)
601		return -(int32_t)ux_dy(dal_fixed31_32_abs(arg).value, 4, 19);
602	else
603		return ux_dy(arg.value, 4, 19);
604}