Loading...
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}
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 const struct fixed31_32 dc_fixpt_two_pi = { 26986075409LL };
30static const struct fixed31_32 dc_fixpt_ln2 = { 2977044471LL };
31static const struct fixed31_32 dc_fixpt_ln2_div_2 = { 1488522236LL };
32
33static 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 */
47static 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
71struct 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
123struct 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 ASSERT(res.value <= LONG_MAX);
144
145 res.value <<= FIXED31_32_BITS_PER_FRACTIONAL_PART;
146
147 tmp = arg1_int * arg2_fra;
148
149 ASSERT(tmp <= (unsigned long long)(LLONG_MAX - res.value));
150
151 res.value += tmp;
152
153 tmp = arg2_int * arg1_fra;
154
155 ASSERT(tmp <= (unsigned long long)(LLONG_MAX - res.value));
156
157 res.value += tmp;
158
159 tmp = arg1_fra * arg2_fra;
160
161 tmp = (tmp >> FIXED31_32_BITS_PER_FRACTIONAL_PART) +
162 (tmp >= (unsigned long long)dc_fixpt_half.value);
163
164 ASSERT(tmp <= (unsigned long long)(LLONG_MAX - res.value));
165
166 res.value += tmp;
167
168 if (arg1_negative ^ arg2_negative)
169 res.value = -res.value;
170
171 return res;
172}
173
174struct fixed31_32 dc_fixpt_sqr(struct fixed31_32 arg)
175{
176 struct fixed31_32 res;
177
178 unsigned long long arg_value = abs_i64(arg.value);
179
180 unsigned long long arg_int = GET_INTEGER_PART(arg_value);
181
182 unsigned long long arg_fra = GET_FRACTIONAL_PART(arg_value);
183
184 unsigned long long tmp;
185
186 res.value = arg_int * arg_int;
187
188 ASSERT(res.value <= LONG_MAX);
189
190 res.value <<= FIXED31_32_BITS_PER_FRACTIONAL_PART;
191
192 tmp = arg_int * arg_fra;
193
194 ASSERT(tmp <= (unsigned long long)(LLONG_MAX - res.value));
195
196 res.value += tmp;
197
198 ASSERT(tmp <= (unsigned long long)(LLONG_MAX - res.value));
199
200 res.value += tmp;
201
202 tmp = arg_fra * arg_fra;
203
204 tmp = (tmp >> FIXED31_32_BITS_PER_FRACTIONAL_PART) +
205 (tmp >= (unsigned long long)dc_fixpt_half.value);
206
207 ASSERT(tmp <= (unsigned long long)(LLONG_MAX - res.value));
208
209 res.value += tmp;
210
211 return res;
212}
213
214struct fixed31_32 dc_fixpt_recip(struct fixed31_32 arg)
215{
216 /*
217 * @note
218 * Good idea to use Newton's method
219 */
220
221 ASSERT(arg.value);
222
223 return dc_fixpt_from_fraction(
224 dc_fixpt_one.value,
225 arg.value);
226}
227
228struct fixed31_32 dc_fixpt_sinc(struct fixed31_32 arg)
229{
230 struct fixed31_32 square;
231
232 struct fixed31_32 res = dc_fixpt_one;
233
234 int n = 27;
235
236 struct fixed31_32 arg_norm = arg;
237
238 if (dc_fixpt_le(
239 dc_fixpt_two_pi,
240 dc_fixpt_abs(arg))) {
241 arg_norm = dc_fixpt_sub(
242 arg_norm,
243 dc_fixpt_mul_int(
244 dc_fixpt_two_pi,
245 (int)div64_s64(
246 arg_norm.value,
247 dc_fixpt_two_pi.value)));
248 }
249
250 square = dc_fixpt_sqr(arg_norm);
251
252 do {
253 res = dc_fixpt_sub(
254 dc_fixpt_one,
255 dc_fixpt_div_int(
256 dc_fixpt_mul(
257 square,
258 res),
259 n * (n - 1)));
260
261 n -= 2;
262 } while (n > 2);
263
264 if (arg.value != arg_norm.value)
265 res = dc_fixpt_div(
266 dc_fixpt_mul(res, arg_norm),
267 arg);
268
269 return res;
270}
271
272struct fixed31_32 dc_fixpt_sin(struct fixed31_32 arg)
273{
274 return dc_fixpt_mul(
275 arg,
276 dc_fixpt_sinc(arg));
277}
278
279struct fixed31_32 dc_fixpt_cos(struct fixed31_32 arg)
280{
281 /* TODO implement argument normalization */
282
283 const struct fixed31_32 square = dc_fixpt_sqr(arg);
284
285 struct fixed31_32 res = dc_fixpt_one;
286
287 int n = 26;
288
289 do {
290 res = dc_fixpt_sub(
291 dc_fixpt_one,
292 dc_fixpt_div_int(
293 dc_fixpt_mul(
294 square,
295 res),
296 n * (n - 1)));
297
298 n -= 2;
299 } while (n != 0);
300
301 return res;
302}
303
304/*
305 * @brief
306 * result = exp(arg),
307 * where abs(arg) < 1
308 *
309 * Calculated as Taylor series.
310 */
311static struct fixed31_32 fixed31_32_exp_from_taylor_series(struct fixed31_32 arg)
312{
313 unsigned int n = 9;
314
315 struct fixed31_32 res = dc_fixpt_from_fraction(
316 n + 2,
317 n + 1);
318 /* TODO find correct res */
319
320 ASSERT(dc_fixpt_lt(arg, dc_fixpt_one));
321
322 do
323 res = dc_fixpt_add(
324 dc_fixpt_one,
325 dc_fixpt_div_int(
326 dc_fixpt_mul(
327 arg,
328 res),
329 n));
330 while (--n != 1);
331
332 return dc_fixpt_add(
333 dc_fixpt_one,
334 dc_fixpt_mul(
335 arg,
336 res));
337}
338
339struct fixed31_32 dc_fixpt_exp(struct fixed31_32 arg)
340{
341 /*
342 * @brief
343 * Main equation is:
344 * exp(x) = exp(r + m * ln(2)) = (1 << m) * exp(r),
345 * where m = round(x / ln(2)), r = x - m * ln(2)
346 */
347
348 if (dc_fixpt_le(
349 dc_fixpt_ln2_div_2,
350 dc_fixpt_abs(arg))) {
351 int m = dc_fixpt_round(
352 dc_fixpt_div(
353 arg,
354 dc_fixpt_ln2));
355
356 struct fixed31_32 r = dc_fixpt_sub(
357 arg,
358 dc_fixpt_mul_int(
359 dc_fixpt_ln2,
360 m));
361
362 ASSERT(m != 0);
363
364 ASSERT(dc_fixpt_lt(
365 dc_fixpt_abs(r),
366 dc_fixpt_one));
367
368 if (m > 0)
369 return dc_fixpt_shl(
370 fixed31_32_exp_from_taylor_series(r),
371 (unsigned char)m);
372 else
373 return dc_fixpt_div_int(
374 fixed31_32_exp_from_taylor_series(r),
375 1LL << -m);
376 } else if (arg.value != 0)
377 return fixed31_32_exp_from_taylor_series(arg);
378 else
379 return dc_fixpt_one;
380}
381
382struct fixed31_32 dc_fixpt_log(struct fixed31_32 arg)
383{
384 struct fixed31_32 res = dc_fixpt_neg(dc_fixpt_one);
385 /* TODO improve 1st estimation */
386
387 struct fixed31_32 error;
388
389 ASSERT(arg.value > 0);
390 /* TODO if arg is negative, return NaN */
391 /* TODO if arg is zero, return -INF */
392
393 do {
394 struct fixed31_32 res1 = dc_fixpt_add(
395 dc_fixpt_sub(
396 res,
397 dc_fixpt_one),
398 dc_fixpt_div(
399 arg,
400 dc_fixpt_exp(res)));
401
402 error = dc_fixpt_sub(
403 res,
404 res1);
405
406 res = res1;
407 /* TODO determine max_allowed_error based on quality of exp() */
408 } while (abs_i64(error.value) > 100ULL);
409
410 return res;
411}
412
413
414/* this function is a generic helper to translate fixed point value to
415 * specified integer format that will consist of integer_bits integer part and
416 * fractional_bits fractional part. For example it is used in
417 * dc_fixpt_u2d19 to receive 2 bits integer part and 19 bits fractional
418 * part in 32 bits. It is used in hw programming (scaler)
419 */
420
421static inline unsigned int ux_dy(
422 long long value,
423 unsigned int integer_bits,
424 unsigned int fractional_bits)
425{
426 /* 1. create mask of integer part */
427 unsigned int result = (1 << integer_bits) - 1;
428 /* 2. mask out fractional part */
429 unsigned int fractional_part = FRACTIONAL_PART_MASK & value;
430 /* 3. shrink fixed point integer part to be of integer_bits width*/
431 result &= GET_INTEGER_PART(value);
432 /* 4. make space for fractional part to be filled in after integer */
433 result <<= fractional_bits;
434 /* 5. shrink fixed point fractional part to of fractional_bits width*/
435 fractional_part >>= FIXED31_32_BITS_PER_FRACTIONAL_PART - fractional_bits;
436 /* 6. merge the result */
437 return result | fractional_part;
438}
439
440static inline unsigned int clamp_ux_dy(
441 long long value,
442 unsigned int integer_bits,
443 unsigned int fractional_bits,
444 unsigned int min_clamp)
445{
446 unsigned int truncated_val = ux_dy(value, integer_bits, fractional_bits);
447
448 if (value >= (1LL << (integer_bits + FIXED31_32_BITS_PER_FRACTIONAL_PART)))
449 return (1 << (integer_bits + fractional_bits)) - 1;
450 else if (truncated_val > min_clamp)
451 return truncated_val;
452 else
453 return min_clamp;
454}
455
456unsigned int dc_fixpt_u4d19(struct fixed31_32 arg)
457{
458 return ux_dy(arg.value, 4, 19);
459}
460
461unsigned int dc_fixpt_u3d19(struct fixed31_32 arg)
462{
463 return ux_dy(arg.value, 3, 19);
464}
465
466unsigned int dc_fixpt_u2d19(struct fixed31_32 arg)
467{
468 return ux_dy(arg.value, 2, 19);
469}
470
471unsigned int dc_fixpt_u0d19(struct fixed31_32 arg)
472{
473 return ux_dy(arg.value, 0, 19);
474}
475
476unsigned int dc_fixpt_clamp_u0d14(struct fixed31_32 arg)
477{
478 return clamp_ux_dy(arg.value, 0, 14, 1);
479}
480
481unsigned int dc_fixpt_clamp_u0d10(struct fixed31_32 arg)
482{
483 return clamp_ux_dy(arg.value, 0, 10, 1);
484}
485
486int dc_fixpt_s4d19(struct fixed31_32 arg)
487{
488 if (arg.value < 0)
489 return -(int)ux_dy(dc_fixpt_abs(arg).value, 4, 19);
490 else
491 return ux_dy(arg.value, 4, 19);
492}