493 lines
11 KiB
C
493 lines
11 KiB
C
/*
|
|
* Copyright 2012-15 Advanced Micro Devices, Inc.
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining a
|
|
* copy of this software and associated documentation files (the "Software"),
|
|
* to deal in the Software without restriction, including without limitation
|
|
* the rights to use, copy, modify, merge, publish, distribute, sublicense,
|
|
* and/or sell copies of the Software, and to permit persons to whom the
|
|
* Software is furnished to do so, subject to the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be included in
|
|
* all copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
|
|
* THE COPYRIGHT HOLDER(S) OR AUTHOR(S) BE LIABLE FOR ANY CLAIM, DAMAGES OR
|
|
* OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
|
|
* ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
|
|
* OTHER DEALINGS IN THE SOFTWARE.
|
|
*
|
|
* Authors: AMD
|
|
*
|
|
*/
|
|
|
|
#include "dm_services.h"
|
|
#include "include/fixed31_32.h"
|
|
|
|
static const struct fixed31_32 dc_fixpt_two_pi = { 26986075409LL };
|
|
static const struct fixed31_32 dc_fixpt_ln2 = { 2977044471LL };
|
|
static const struct fixed31_32 dc_fixpt_ln2_div_2 = { 1488522236LL };
|
|
|
|
static inline unsigned long long abs_i64(
|
|
long long arg)
|
|
{
|
|
if (arg > 0)
|
|
return (unsigned long long)arg;
|
|
else
|
|
return (unsigned long long)(-arg);
|
|
}
|
|
|
|
/*
|
|
* @brief
|
|
* result = dividend / divisor
|
|
* *remainder = dividend % divisor
|
|
*/
|
|
static inline unsigned long long complete_integer_division_u64(
|
|
unsigned long long dividend,
|
|
unsigned long long divisor,
|
|
unsigned long long *remainder)
|
|
{
|
|
unsigned long long result;
|
|
|
|
ASSERT(divisor);
|
|
|
|
result = div64_u64_rem(dividend, divisor, remainder);
|
|
|
|
return result;
|
|
}
|
|
|
|
|
|
#define FRACTIONAL_PART_MASK \
|
|
((1ULL << FIXED31_32_BITS_PER_FRACTIONAL_PART) - 1)
|
|
|
|
#define GET_INTEGER_PART(x) \
|
|
((x) >> FIXED31_32_BITS_PER_FRACTIONAL_PART)
|
|
|
|
#define GET_FRACTIONAL_PART(x) \
|
|
(FRACTIONAL_PART_MASK & (x))
|
|
|
|
struct fixed31_32 dc_fixpt_from_fraction(long long numerator, long long denominator)
|
|
{
|
|
struct fixed31_32 res;
|
|
|
|
bool arg1_negative = numerator < 0;
|
|
bool arg2_negative = denominator < 0;
|
|
|
|
unsigned long long arg1_value = arg1_negative ? -numerator : numerator;
|
|
unsigned long long arg2_value = arg2_negative ? -denominator : denominator;
|
|
|
|
unsigned long long remainder;
|
|
|
|
/* determine integer part */
|
|
|
|
unsigned long long res_value = complete_integer_division_u64(
|
|
arg1_value, arg2_value, &remainder);
|
|
|
|
ASSERT(res_value <= LONG_MAX);
|
|
|
|
/* determine fractional part */
|
|
{
|
|
unsigned int i = FIXED31_32_BITS_PER_FRACTIONAL_PART;
|
|
|
|
do {
|
|
remainder <<= 1;
|
|
|
|
res_value <<= 1;
|
|
|
|
if (remainder >= arg2_value) {
|
|
res_value |= 1;
|
|
remainder -= arg2_value;
|
|
}
|
|
} while (--i != 0);
|
|
}
|
|
|
|
/* round up LSB */
|
|
{
|
|
unsigned long long summand = (remainder << 1) >= arg2_value;
|
|
|
|
ASSERT(res_value <= LLONG_MAX - summand);
|
|
|
|
res_value += summand;
|
|
}
|
|
|
|
res.value = (long long)res_value;
|
|
|
|
if (arg1_negative ^ arg2_negative)
|
|
res.value = -res.value;
|
|
|
|
return res;
|
|
}
|
|
|
|
struct fixed31_32 dc_fixpt_mul(struct fixed31_32 arg1, struct fixed31_32 arg2)
|
|
{
|
|
struct fixed31_32 res;
|
|
|
|
bool arg1_negative = arg1.value < 0;
|
|
bool arg2_negative = arg2.value < 0;
|
|
|
|
unsigned long long arg1_value = arg1_negative ? -arg1.value : arg1.value;
|
|
unsigned long long arg2_value = arg2_negative ? -arg2.value : arg2.value;
|
|
|
|
unsigned long long arg1_int = GET_INTEGER_PART(arg1_value);
|
|
unsigned long long arg2_int = GET_INTEGER_PART(arg2_value);
|
|
|
|
unsigned long long arg1_fra = GET_FRACTIONAL_PART(arg1_value);
|
|
unsigned long long arg2_fra = GET_FRACTIONAL_PART(arg2_value);
|
|
|
|
unsigned long long tmp;
|
|
|
|
res.value = arg1_int * arg2_int;
|
|
|
|
ASSERT(res.value <= LONG_MAX);
|
|
|
|
res.value <<= FIXED31_32_BITS_PER_FRACTIONAL_PART;
|
|
|
|
tmp = arg1_int * arg2_fra;
|
|
|
|
ASSERT(tmp <= (unsigned long long)(LLONG_MAX - res.value));
|
|
|
|
res.value += tmp;
|
|
|
|
tmp = arg2_int * arg1_fra;
|
|
|
|
ASSERT(tmp <= (unsigned long long)(LLONG_MAX - res.value));
|
|
|
|
res.value += tmp;
|
|
|
|
tmp = arg1_fra * arg2_fra;
|
|
|
|
tmp = (tmp >> FIXED31_32_BITS_PER_FRACTIONAL_PART) +
|
|
(tmp >= (unsigned long long)dc_fixpt_half.value);
|
|
|
|
ASSERT(tmp <= (unsigned long long)(LLONG_MAX - res.value));
|
|
|
|
res.value += tmp;
|
|
|
|
if (arg1_negative ^ arg2_negative)
|
|
res.value = -res.value;
|
|
|
|
return res;
|
|
}
|
|
|
|
struct fixed31_32 dc_fixpt_sqr(struct fixed31_32 arg)
|
|
{
|
|
struct fixed31_32 res;
|
|
|
|
unsigned long long arg_value = abs_i64(arg.value);
|
|
|
|
unsigned long long arg_int = GET_INTEGER_PART(arg_value);
|
|
|
|
unsigned long long arg_fra = GET_FRACTIONAL_PART(arg_value);
|
|
|
|
unsigned long long tmp;
|
|
|
|
res.value = arg_int * arg_int;
|
|
|
|
ASSERT(res.value <= LONG_MAX);
|
|
|
|
res.value <<= FIXED31_32_BITS_PER_FRACTIONAL_PART;
|
|
|
|
tmp = arg_int * arg_fra;
|
|
|
|
ASSERT(tmp <= (unsigned long long)(LLONG_MAX - res.value));
|
|
|
|
res.value += tmp;
|
|
|
|
ASSERT(tmp <= (unsigned long long)(LLONG_MAX - res.value));
|
|
|
|
res.value += tmp;
|
|
|
|
tmp = arg_fra * arg_fra;
|
|
|
|
tmp = (tmp >> FIXED31_32_BITS_PER_FRACTIONAL_PART) +
|
|
(tmp >= (unsigned long long)dc_fixpt_half.value);
|
|
|
|
ASSERT(tmp <= (unsigned long long)(LLONG_MAX - res.value));
|
|
|
|
res.value += tmp;
|
|
|
|
return res;
|
|
}
|
|
|
|
struct fixed31_32 dc_fixpt_recip(struct fixed31_32 arg)
|
|
{
|
|
/*
|
|
* @note
|
|
* Good idea to use Newton's method
|
|
*/
|
|
|
|
ASSERT(arg.value);
|
|
|
|
return dc_fixpt_from_fraction(
|
|
dc_fixpt_one.value,
|
|
arg.value);
|
|
}
|
|
|
|
struct fixed31_32 dc_fixpt_sinc(struct fixed31_32 arg)
|
|
{
|
|
struct fixed31_32 square;
|
|
|
|
struct fixed31_32 res = dc_fixpt_one;
|
|
|
|
int n = 27;
|
|
|
|
struct fixed31_32 arg_norm = arg;
|
|
|
|
if (dc_fixpt_le(
|
|
dc_fixpt_two_pi,
|
|
dc_fixpt_abs(arg))) {
|
|
arg_norm = dc_fixpt_sub(
|
|
arg_norm,
|
|
dc_fixpt_mul_int(
|
|
dc_fixpt_two_pi,
|
|
(int)div64_s64(
|
|
arg_norm.value,
|
|
dc_fixpt_two_pi.value)));
|
|
}
|
|
|
|
square = dc_fixpt_sqr(arg_norm);
|
|
|
|
do {
|
|
res = dc_fixpt_sub(
|
|
dc_fixpt_one,
|
|
dc_fixpt_div_int(
|
|
dc_fixpt_mul(
|
|
square,
|
|
res),
|
|
n * (n - 1)));
|
|
|
|
n -= 2;
|
|
} while (n > 2);
|
|
|
|
if (arg.value != arg_norm.value)
|
|
res = dc_fixpt_div(
|
|
dc_fixpt_mul(res, arg_norm),
|
|
arg);
|
|
|
|
return res;
|
|
}
|
|
|
|
struct fixed31_32 dc_fixpt_sin(struct fixed31_32 arg)
|
|
{
|
|
return dc_fixpt_mul(
|
|
arg,
|
|
dc_fixpt_sinc(arg));
|
|
}
|
|
|
|
struct fixed31_32 dc_fixpt_cos(struct fixed31_32 arg)
|
|
{
|
|
/* TODO implement argument normalization */
|
|
|
|
const struct fixed31_32 square = dc_fixpt_sqr(arg);
|
|
|
|
struct fixed31_32 res = dc_fixpt_one;
|
|
|
|
int n = 26;
|
|
|
|
do {
|
|
res = dc_fixpt_sub(
|
|
dc_fixpt_one,
|
|
dc_fixpt_div_int(
|
|
dc_fixpt_mul(
|
|
square,
|
|
res),
|
|
n * (n - 1)));
|
|
|
|
n -= 2;
|
|
} while (n != 0);
|
|
|
|
return res;
|
|
}
|
|
|
|
/*
|
|
* @brief
|
|
* result = exp(arg),
|
|
* where abs(arg) < 1
|
|
*
|
|
* Calculated as Taylor series.
|
|
*/
|
|
static struct fixed31_32 fixed31_32_exp_from_taylor_series(struct fixed31_32 arg)
|
|
{
|
|
unsigned int n = 9;
|
|
|
|
struct fixed31_32 res = dc_fixpt_from_fraction(
|
|
n + 2,
|
|
n + 1);
|
|
/* TODO find correct res */
|
|
|
|
ASSERT(dc_fixpt_lt(arg, dc_fixpt_one));
|
|
|
|
do
|
|
res = dc_fixpt_add(
|
|
dc_fixpt_one,
|
|
dc_fixpt_div_int(
|
|
dc_fixpt_mul(
|
|
arg,
|
|
res),
|
|
n));
|
|
while (--n != 1);
|
|
|
|
return dc_fixpt_add(
|
|
dc_fixpt_one,
|
|
dc_fixpt_mul(
|
|
arg,
|
|
res));
|
|
}
|
|
|
|
struct fixed31_32 dc_fixpt_exp(struct fixed31_32 arg)
|
|
{
|
|
/*
|
|
* @brief
|
|
* Main equation is:
|
|
* exp(x) = exp(r + m * ln(2)) = (1 << m) * exp(r),
|
|
* where m = round(x / ln(2)), r = x - m * ln(2)
|
|
*/
|
|
|
|
if (dc_fixpt_le(
|
|
dc_fixpt_ln2_div_2,
|
|
dc_fixpt_abs(arg))) {
|
|
int m = dc_fixpt_round(
|
|
dc_fixpt_div(
|
|
arg,
|
|
dc_fixpt_ln2));
|
|
|
|
struct fixed31_32 r = dc_fixpt_sub(
|
|
arg,
|
|
dc_fixpt_mul_int(
|
|
dc_fixpt_ln2,
|
|
m));
|
|
|
|
ASSERT(m != 0);
|
|
|
|
ASSERT(dc_fixpt_lt(
|
|
dc_fixpt_abs(r),
|
|
dc_fixpt_one));
|
|
|
|
if (m > 0)
|
|
return dc_fixpt_shl(
|
|
fixed31_32_exp_from_taylor_series(r),
|
|
(unsigned char)m);
|
|
else
|
|
return dc_fixpt_div_int(
|
|
fixed31_32_exp_from_taylor_series(r),
|
|
1LL << -m);
|
|
} else if (arg.value != 0)
|
|
return fixed31_32_exp_from_taylor_series(arg);
|
|
else
|
|
return dc_fixpt_one;
|
|
}
|
|
|
|
struct fixed31_32 dc_fixpt_log(struct fixed31_32 arg)
|
|
{
|
|
struct fixed31_32 res = dc_fixpt_neg(dc_fixpt_one);
|
|
/* TODO improve 1st estimation */
|
|
|
|
struct fixed31_32 error;
|
|
|
|
ASSERT(arg.value > 0);
|
|
/* TODO if arg is negative, return NaN */
|
|
/* TODO if arg is zero, return -INF */
|
|
|
|
do {
|
|
struct fixed31_32 res1 = dc_fixpt_add(
|
|
dc_fixpt_sub(
|
|
res,
|
|
dc_fixpt_one),
|
|
dc_fixpt_div(
|
|
arg,
|
|
dc_fixpt_exp(res)));
|
|
|
|
error = dc_fixpt_sub(
|
|
res,
|
|
res1);
|
|
|
|
res = res1;
|
|
/* TODO determine max_allowed_error based on quality of exp() */
|
|
} while (abs_i64(error.value) > 100ULL);
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
/* this function is a generic helper to translate fixed point value to
|
|
* specified integer format that will consist of integer_bits integer part and
|
|
* fractional_bits fractional part. For example it is used in
|
|
* dc_fixpt_u2d19 to receive 2 bits integer part and 19 bits fractional
|
|
* part in 32 bits. It is used in hw programming (scaler)
|
|
*/
|
|
|
|
static inline unsigned int ux_dy(
|
|
long long value,
|
|
unsigned int integer_bits,
|
|
unsigned int fractional_bits)
|
|
{
|
|
/* 1. create mask of integer part */
|
|
unsigned int result = (1 << integer_bits) - 1;
|
|
/* 2. mask out fractional part */
|
|
unsigned int fractional_part = FRACTIONAL_PART_MASK & value;
|
|
/* 3. shrink fixed point integer part to be of integer_bits width*/
|
|
result &= GET_INTEGER_PART(value);
|
|
/* 4. make space for fractional part to be filled in after integer */
|
|
result <<= fractional_bits;
|
|
/* 5. shrink fixed point fractional part to of fractional_bits width*/
|
|
fractional_part >>= FIXED31_32_BITS_PER_FRACTIONAL_PART - fractional_bits;
|
|
/* 6. merge the result */
|
|
return result | fractional_part;
|
|
}
|
|
|
|
static inline unsigned int clamp_ux_dy(
|
|
long long value,
|
|
unsigned int integer_bits,
|
|
unsigned int fractional_bits,
|
|
unsigned int min_clamp)
|
|
{
|
|
unsigned int truncated_val = ux_dy(value, integer_bits, fractional_bits);
|
|
|
|
if (value >= (1LL << (integer_bits + FIXED31_32_BITS_PER_FRACTIONAL_PART)))
|
|
return (1 << (integer_bits + fractional_bits)) - 1;
|
|
else if (truncated_val > min_clamp)
|
|
return truncated_val;
|
|
else
|
|
return min_clamp;
|
|
}
|
|
|
|
unsigned int dc_fixpt_u4d19(struct fixed31_32 arg)
|
|
{
|
|
return ux_dy(arg.value, 4, 19);
|
|
}
|
|
|
|
unsigned int dc_fixpt_u3d19(struct fixed31_32 arg)
|
|
{
|
|
return ux_dy(arg.value, 3, 19);
|
|
}
|
|
|
|
unsigned int dc_fixpt_u2d19(struct fixed31_32 arg)
|
|
{
|
|
return ux_dy(arg.value, 2, 19);
|
|
}
|
|
|
|
unsigned int dc_fixpt_u0d19(struct fixed31_32 arg)
|
|
{
|
|
return ux_dy(arg.value, 0, 19);
|
|
}
|
|
|
|
unsigned int dc_fixpt_clamp_u0d14(struct fixed31_32 arg)
|
|
{
|
|
return clamp_ux_dy(arg.value, 0, 14, 1);
|
|
}
|
|
|
|
unsigned int dc_fixpt_clamp_u0d10(struct fixed31_32 arg)
|
|
{
|
|
return clamp_ux_dy(arg.value, 0, 10, 1);
|
|
}
|
|
|
|
int dc_fixpt_s4d19(struct fixed31_32 arg)
|
|
{
|
|
if (arg.value < 0)
|
|
return -(int)ux_dy(dc_fixpt_abs(arg).value, 4, 19);
|
|
else
|
|
return ux_dy(arg.value, 4, 19);
|
|
}
|