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