forked from rrcarlosr/Jetpack
601 lines
14 KiB
C
601 lines
14 KiB
C
/*
|
|
* Copyright (C) 2017-2018, NVIDIA CORPORATION. All rights reserved.
|
|
*
|
|
* This software is licensed under the terms of the GNU General Public
|
|
* License version 2, as published by the Free Software Foundation, and
|
|
* may be copied, distributed, and modified under those terms.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
*/
|
|
#include <asm/bug.h>
|
|
#include "fixed_point.h"
|
|
|
|
|
|
#define FIX_PT_CHK_MATCH(chk_type, op1, op2) \
|
|
do { \
|
|
if (op1 != op2) { \
|
|
pr_err("%s: op %s do not match: %d != %d\n", \
|
|
__func__, chk_type, op1, op2); \
|
|
(*error) |= 1; \
|
|
WARN_ON(1); \
|
|
} \
|
|
} while (0)
|
|
|
|
#define FIX_PT_CHK_MATCH_PREC(prec1, prec2) \
|
|
FIX_PT_CHK_MATCH("precisions", prec1, prec2)
|
|
|
|
#define FIX_PT_CHK_MATCH_MASK(prec1, prec2) \
|
|
FIX_PT_CHK_MATCH("masks", prec1, prec2)
|
|
|
|
struct fixed_point fixed_point_init(
|
|
unsigned int int_part,
|
|
unsigned int frac_part,
|
|
unsigned int int_prec,
|
|
unsigned int frac_prec,
|
|
unsigned int *error)
|
|
{
|
|
struct fixed_point ret_fp;
|
|
unsigned int max_prec = (sizeof(unsigned int) * 8);
|
|
|
|
if (int_prec > max_prec) {
|
|
pr_err("%s: int_prec too large: %d > %d\n",
|
|
__func__, int_prec, max_prec);
|
|
(*error) |= 1;
|
|
WARN_ON(1);
|
|
} else if (int_prec == (sizeof(unsigned int) * 8))
|
|
ret_fp.int_mask = ((unsigned int)(-1));
|
|
else
|
|
ret_fp.int_mask = (1 << int_prec) - 1;
|
|
|
|
if (frac_prec > max_prec) {
|
|
pr_err("%s: frac_prec too large: %d > %d\n",
|
|
__func__, frac_prec, max_prec);
|
|
(*error) |= 1;
|
|
WARN_ON(1);
|
|
} else if (frac_prec == (sizeof(unsigned int) * 8))
|
|
ret_fp.frac_mask = ((unsigned int)(-1));
|
|
else
|
|
ret_fp.frac_mask = (1 << frac_prec) - 1;
|
|
|
|
ret_fp.int_part = int_part & ret_fp.int_mask;
|
|
ret_fp.frac_part = frac_part & ret_fp.frac_mask;
|
|
ret_fp.int_prec = int_prec;
|
|
ret_fp.frac_prec = frac_prec;
|
|
|
|
return ret_fp;
|
|
}
|
|
|
|
struct fixed_point fixed_point_shift_left(
|
|
struct fixed_point fp_arg,
|
|
unsigned int places,
|
|
unsigned int *error)
|
|
{
|
|
struct fixed_point ret_fp;
|
|
unsigned int frac_mask;
|
|
unsigned int frac_shift;
|
|
|
|
if (places == 0)
|
|
return fp_arg;
|
|
|
|
ret_fp = fixed_point_init(
|
|
0,
|
|
0,
|
|
fp_arg.int_prec,
|
|
fp_arg.frac_prec,
|
|
error);
|
|
|
|
frac_mask = (places >= fp_arg.frac_prec) ? 0 :
|
|
(((((unsigned int)(1 << places)) - 1) << (fp_arg.frac_prec - places)) &
|
|
fp_arg.frac_mask);
|
|
frac_shift = (places >= fp_arg.frac_prec) ? 0 :
|
|
(fp_arg.frac_prec - places);
|
|
|
|
ret_fp.int_part = (places < fp_arg.int_prec) ?
|
|
((fp_arg.int_part << places) & fp_arg.int_mask) : 0;
|
|
ret_fp.int_part |= ((fp_arg.frac_part & frac_mask) >> frac_shift);
|
|
ret_fp.frac_part = (places < fp_arg.frac_prec) ?
|
|
((fp_arg.frac_part << places) & fp_arg.frac_mask) : 0;
|
|
|
|
return ret_fp;
|
|
}
|
|
|
|
struct fixed_point fixed_point_shift_right(
|
|
struct fixed_point fp_arg,
|
|
unsigned int places,
|
|
unsigned int *error)
|
|
{
|
|
struct fixed_point ret_fp;
|
|
unsigned int int_mask = 0;
|
|
unsigned int frac_shift = 0;
|
|
int sign_ext = 0;
|
|
|
|
if (places == 0)
|
|
return fp_arg;
|
|
|
|
ret_fp = fixed_point_init(
|
|
0,
|
|
0,
|
|
fp_arg.int_prec,
|
|
fp_arg.frac_prec,
|
|
error);
|
|
|
|
ret_fp.int_part = (places < fp_arg.int_prec) ?
|
|
((fp_arg.int_part >> places) & fp_arg.int_mask) : 0;
|
|
if (fp_arg.int_part & (1 << (fp_arg.int_prec - 1))) { /* sign extend */
|
|
sign_ext = 1;
|
|
if (places < fp_arg.int_prec) {
|
|
ret_fp.int_part |=
|
|
((((1 << places) - 1) <<
|
|
(fp_arg.int_prec - places)) & fp_arg.int_mask);
|
|
} else {
|
|
ret_fp.int_part =
|
|
((unsigned int)(-1)) & ret_fp.int_mask;
|
|
}
|
|
}
|
|
ret_fp.frac_part = (places < fp_arg.frac_prec) ?
|
|
((fp_arg.frac_part >> places) & fp_arg.frac_mask) : 0;
|
|
|
|
if (places < fp_arg.frac_prec) {
|
|
int_mask = fp_arg.int_mask >> (fp_arg.int_prec - places);
|
|
frac_shift = fp_arg.frac_prec - places;
|
|
ret_fp.frac_part |=
|
|
(((fp_arg.int_part & int_mask) << frac_shift) &
|
|
fp_arg.frac_mask);
|
|
} else if (places < (fp_arg.int_prec + fp_arg.frac_prec)) {
|
|
int_mask = (fp_arg.int_mask << (places - fp_arg.frac_prec)) &
|
|
fp_arg.int_mask;
|
|
frac_shift = places - fp_arg.frac_prec;
|
|
ret_fp.frac_part |=
|
|
(((fp_arg.int_part & int_mask) >> frac_shift) &
|
|
fp_arg.frac_mask);
|
|
if (sign_ext) {
|
|
ret_fp.frac_part |=
|
|
(fp_arg.frac_mask <<
|
|
(fp_arg.frac_prec -
|
|
(places - fp_arg.frac_prec)));
|
|
}
|
|
} else {
|
|
if (sign_ext)
|
|
ret_fp.frac_part = fp_arg.frac_mask;
|
|
}
|
|
|
|
|
|
return ret_fp;
|
|
}
|
|
|
|
struct fixed_point fixed_point_negate(
|
|
struct fixed_point fp_arg,
|
|
unsigned int *error)
|
|
{
|
|
struct fixed_point ret_fp;
|
|
struct fixed_point fp_one;
|
|
|
|
ret_fp = fixed_point_init(
|
|
0,
|
|
0,
|
|
fp_arg.int_prec,
|
|
fp_arg.frac_prec,
|
|
error);
|
|
|
|
ret_fp.int_part = ~fp_arg.int_part;
|
|
ret_fp.frac_part = ~fp_arg.frac_part;
|
|
|
|
fp_one.int_prec = fp_arg.int_prec;
|
|
fp_one.frac_prec = fp_arg.frac_prec;
|
|
fp_one.int_mask = fp_arg.int_mask;
|
|
fp_one.frac_mask = fp_arg.frac_mask;
|
|
|
|
fp_one.int_part = 0;
|
|
fp_one.frac_part = 1;
|
|
|
|
ret_fp = fixed_point_add(ret_fp, fp_one, error);
|
|
|
|
return ret_fp;
|
|
}
|
|
|
|
struct fixed_point fixed_point_add(
|
|
struct fixed_point fp_arg1,
|
|
struct fixed_point fp_arg2,
|
|
unsigned int *error)
|
|
{
|
|
struct fixed_point ret_fp;
|
|
unsigned int frac_carry_out;
|
|
unsigned int frac1_msb;
|
|
unsigned int frac2_msb;
|
|
unsigned int sum_msb;
|
|
|
|
FIX_PT_CHK_MATCH_PREC(
|
|
fp_arg1.int_prec,
|
|
fp_arg2.int_prec);
|
|
FIX_PT_CHK_MATCH_PREC(
|
|
fp_arg1.frac_prec,
|
|
fp_arg2.frac_prec);
|
|
FIX_PT_CHK_MATCH_MASK(
|
|
fp_arg1.int_mask,
|
|
fp_arg2.int_mask);
|
|
FIX_PT_CHK_MATCH_MASK(
|
|
fp_arg1.frac_mask,
|
|
fp_arg2.frac_mask);
|
|
|
|
ret_fp = fixed_point_init(
|
|
0,
|
|
0,
|
|
fp_arg1.int_prec,
|
|
fp_arg1.frac_prec,
|
|
error);
|
|
|
|
ret_fp.frac_part = (fp_arg1.frac_part + fp_arg2.frac_part) &
|
|
fp_arg1.frac_mask;
|
|
|
|
frac1_msb = (fp_arg1.frac_part >> (fp_arg1.frac_prec - 1)) & 1;
|
|
frac2_msb = (fp_arg2.frac_part >> (fp_arg2.frac_prec - 1)) & 1;
|
|
sum_msb = (ret_fp.frac_part >> (fp_arg1.frac_prec - 1)) & 1;
|
|
frac_carry_out = ((frac1_msb & frac2_msb) |
|
|
((frac1_msb | frac2_msb) & ~sum_msb)) & 1;
|
|
|
|
ret_fp.int_part = (fp_arg1.int_part +
|
|
fp_arg2.int_part +
|
|
frac_carry_out) & fp_arg1.int_mask;
|
|
|
|
return ret_fp;
|
|
}
|
|
|
|
struct fixed_point fixed_point_sub(
|
|
struct fixed_point minuend_arg,
|
|
struct fixed_point subtrahend_arg,
|
|
unsigned int *error)
|
|
{
|
|
struct fixed_point neg_subtrahend =
|
|
fixed_point_negate(subtrahend_arg, error);
|
|
|
|
return fixed_point_add(minuend_arg, neg_subtrahend, error);
|
|
}
|
|
|
|
struct fixed_point fixed_point_mult(
|
|
struct fixed_point fp_arg1,
|
|
struct fixed_point fp_arg2,
|
|
unsigned int *error)
|
|
{
|
|
struct fixed_point ret_fp;
|
|
struct fixed_point tmp_fp1;
|
|
struct fixed_point tmp_fp2;
|
|
int i;
|
|
|
|
FIX_PT_CHK_MATCH_PREC(
|
|
fp_arg1.int_prec,
|
|
fp_arg2.int_prec);
|
|
FIX_PT_CHK_MATCH_PREC(
|
|
fp_arg1.frac_prec,
|
|
fp_arg2.frac_prec);
|
|
FIX_PT_CHK_MATCH_MASK(
|
|
fp_arg1.int_mask,
|
|
fp_arg2.int_mask);
|
|
FIX_PT_CHK_MATCH_MASK(
|
|
fp_arg1.frac_mask,
|
|
fp_arg2.frac_mask);
|
|
|
|
ret_fp = fixed_point_init(
|
|
0,
|
|
0,
|
|
fp_arg1.int_prec,
|
|
fp_arg1.frac_prec,
|
|
error);
|
|
|
|
tmp_fp2 = fp_arg2;
|
|
|
|
for (i = 0; i < fp_arg2.frac_prec; i++) {
|
|
ret_fp = fixed_point_shift_right(ret_fp, 1, error);
|
|
if (tmp_fp2.frac_part & 1)
|
|
ret_fp = fixed_point_add(ret_fp, fp_arg1, error);
|
|
tmp_fp2 = fixed_point_shift_right(tmp_fp2, 1, error);
|
|
}
|
|
|
|
ret_fp = fixed_point_shift_right(ret_fp, 1, error);
|
|
tmp_fp1 = fp_arg1;
|
|
for (i = fp_arg2.frac_prec;
|
|
i < (fp_arg2.int_prec + fp_arg2.frac_prec - 1); i++) {
|
|
if (tmp_fp2.frac_part & 1)
|
|
ret_fp = fixed_point_add(ret_fp, tmp_fp1, error);
|
|
tmp_fp2 = fixed_point_shift_right(tmp_fp2, 1, error);
|
|
tmp_fp1 = fixed_point_shift_left(tmp_fp1, 1, error);
|
|
}
|
|
|
|
if (tmp_fp2.frac_part & 1)
|
|
ret_fp = fixed_point_sub(ret_fp, tmp_fp1, error);
|
|
|
|
return ret_fp;
|
|
}
|
|
|
|
struct fixed_point fixed_point_div(
|
|
struct fixed_point dividend_arg,
|
|
struct fixed_point divisor_arg,
|
|
unsigned int *error)
|
|
{
|
|
struct fixed_point ret_fp;
|
|
struct fixed_point tmp_dividend;
|
|
struct fixed_point tmp_divisor;
|
|
struct fixed_point tmp_accum;
|
|
int negate_num = 0;
|
|
int i;
|
|
|
|
FIX_PT_CHK_MATCH_PREC(
|
|
dividend_arg.int_prec,
|
|
divisor_arg.int_prec);
|
|
FIX_PT_CHK_MATCH_PREC(
|
|
dividend_arg.frac_prec,
|
|
divisor_arg.frac_prec);
|
|
FIX_PT_CHK_MATCH_MASK(
|
|
dividend_arg.int_mask,
|
|
divisor_arg.int_mask);
|
|
FIX_PT_CHK_MATCH_MASK(
|
|
dividend_arg.frac_mask,
|
|
divisor_arg.frac_mask);
|
|
|
|
if (fixed_point_eq(divisor_arg,
|
|
fixed_point_init(
|
|
0,
|
|
0,
|
|
dividend_arg.int_prec,
|
|
dividend_arg.frac_prec,
|
|
error),
|
|
error
|
|
)
|
|
){
|
|
|
|
if (dividend_arg.int_part &
|
|
(1 << (dividend_arg.int_prec - 1))) {
|
|
ret_fp = fixed_point_init(
|
|
1 << (dividend_arg.int_prec - 1),
|
|
0,
|
|
dividend_arg.int_prec,
|
|
dividend_arg.frac_prec,
|
|
error);
|
|
} else {
|
|
ret_fp = fixed_point_init(
|
|
(1 << (dividend_arg.int_prec - 1)) - 1,
|
|
dividend_arg.frac_mask,
|
|
dividend_arg.int_prec,
|
|
dividend_arg.frac_prec,
|
|
error);
|
|
}
|
|
|
|
return ret_fp;
|
|
}
|
|
|
|
|
|
ret_fp = fixed_point_init(
|
|
0,
|
|
0,
|
|
dividend_arg.int_prec,
|
|
dividend_arg.frac_prec,
|
|
error);
|
|
|
|
tmp_accum = fixed_point_init(
|
|
0,
|
|
0,
|
|
dividend_arg.int_prec,
|
|
dividend_arg.frac_prec,
|
|
error);
|
|
|
|
if (dividend_arg.int_part & (1 << (dividend_arg.int_prec - 1))) {
|
|
tmp_dividend = fixed_point_negate(dividend_arg, error);
|
|
negate_num++;
|
|
} else {
|
|
tmp_dividend = dividend_arg;
|
|
}
|
|
|
|
if (divisor_arg.int_part & (1 << (dividend_arg.int_prec - 1))) {
|
|
tmp_divisor = fixed_point_negate(divisor_arg, error);
|
|
negate_num++;
|
|
} else {
|
|
tmp_divisor = divisor_arg;
|
|
}
|
|
|
|
negate_num = negate_num % 2;
|
|
|
|
for (i = 0;
|
|
i < (dividend_arg.int_prec + dividend_arg.frac_prec);
|
|
i++) {
|
|
struct fixed_point next_dividend_shifted =
|
|
fixed_point_shift_right(tmp_dividend,
|
|
dividend_arg.int_prec +
|
|
dividend_arg.frac_prec - i - 1,
|
|
error);
|
|
tmp_accum = fixed_point_shift_left(tmp_accum, 1, error);
|
|
tmp_accum.frac_part |= (next_dividend_shifted.frac_part & 1);
|
|
|
|
ret_fp = fixed_point_shift_left(ret_fp, 1, error);
|
|
if (fixed_point_loet(tmp_divisor, tmp_accum, error)) {
|
|
tmp_accum =
|
|
fixed_point_sub(tmp_accum, tmp_divisor, error);
|
|
ret_fp.frac_part |= 1;
|
|
}
|
|
}
|
|
|
|
for (i = 0;
|
|
i < dividend_arg.frac_prec;
|
|
i++) {
|
|
struct fixed_point next_dividend_shifted =
|
|
fixed_point_shift_left(tmp_dividend,
|
|
i + 1,
|
|
error);
|
|
tmp_accum = fixed_point_shift_left(tmp_accum, 1, error);
|
|
tmp_accum.frac_part |= (next_dividend_shifted.frac_part & 1);
|
|
|
|
ret_fp = fixed_point_shift_left(ret_fp, 1, error);
|
|
if (fixed_point_loet(tmp_divisor, tmp_accum, error)) {
|
|
tmp_accum =
|
|
fixed_point_sub(tmp_accum, tmp_divisor, error);
|
|
ret_fp.frac_part |= 1;
|
|
}
|
|
}
|
|
|
|
if (negate_num)
|
|
ret_fp = fixed_point_negate(ret_fp, error);
|
|
|
|
return ret_fp;
|
|
}
|
|
|
|
int fixed_point_lt(
|
|
struct fixed_point fp_lhs_arg,
|
|
struct fixed_point fp_rhs_arg,
|
|
unsigned int *error)
|
|
{
|
|
unsigned int int_lhs_msb;
|
|
unsigned int int_rhs_msb;
|
|
|
|
FIX_PT_CHK_MATCH_PREC(
|
|
fp_lhs_arg.int_prec,
|
|
fp_rhs_arg.int_prec);
|
|
FIX_PT_CHK_MATCH_PREC(
|
|
fp_lhs_arg.frac_prec,
|
|
fp_rhs_arg.frac_prec);
|
|
FIX_PT_CHK_MATCH_MASK(
|
|
fp_lhs_arg.int_mask,
|
|
fp_rhs_arg.int_mask);
|
|
FIX_PT_CHK_MATCH_MASK(
|
|
fp_lhs_arg.frac_mask,
|
|
fp_rhs_arg.frac_mask);
|
|
|
|
int_lhs_msb = (fp_lhs_arg.int_part >> (fp_lhs_arg.int_prec - 1)) & 1;
|
|
int_rhs_msb = (fp_rhs_arg.int_part >> (fp_rhs_arg.int_prec - 1)) & 1;
|
|
|
|
if ((int_lhs_msb == 1) && (int_rhs_msb == 0))
|
|
return 1;
|
|
|
|
if ((int_lhs_msb == 0) && (int_rhs_msb == 1))
|
|
return 0;
|
|
|
|
/* both are positive or both are negative */
|
|
if (fp_lhs_arg.int_part < fp_rhs_arg.int_part)
|
|
return 1;
|
|
|
|
if (fp_lhs_arg.int_part > fp_rhs_arg.int_part)
|
|
return 0;
|
|
|
|
if (fp_lhs_arg.frac_part < fp_rhs_arg.frac_part)
|
|
return 1;
|
|
|
|
if (fp_lhs_arg.frac_part > fp_rhs_arg.frac_part)
|
|
return 0;
|
|
|
|
/* equal */
|
|
return 0;
|
|
}
|
|
|
|
int fixed_point_gt(
|
|
struct fixed_point fp_lhs_arg,
|
|
struct fixed_point fp_rhs_arg,
|
|
unsigned int *error)
|
|
{
|
|
|
|
return fixed_point_lt(fp_rhs_arg, fp_lhs_arg, error);
|
|
}
|
|
|
|
int fixed_point_loet(
|
|
struct fixed_point fp_lhs_arg,
|
|
struct fixed_point fp_rhs_arg,
|
|
unsigned int *error)
|
|
{
|
|
return fixed_point_lt(fp_lhs_arg, fp_rhs_arg, error) |
|
|
fixed_point_eq(fp_lhs_arg, fp_rhs_arg, error);
|
|
}
|
|
|
|
int fixed_point_goet(
|
|
struct fixed_point fp_lhs_arg,
|
|
struct fixed_point fp_rhs_arg,
|
|
unsigned int *error)
|
|
{
|
|
return fixed_point_gt(fp_lhs_arg, fp_rhs_arg, error) |
|
|
fixed_point_eq(fp_lhs_arg, fp_rhs_arg, error);
|
|
}
|
|
|
|
int fixed_point_eq(
|
|
struct fixed_point fp_lhs_arg,
|
|
struct fixed_point fp_rhs_arg,
|
|
unsigned int *error)
|
|
{
|
|
FIX_PT_CHK_MATCH_PREC(
|
|
fp_lhs_arg.int_prec,
|
|
fp_rhs_arg.int_prec);
|
|
FIX_PT_CHK_MATCH_PREC(
|
|
fp_lhs_arg.frac_prec,
|
|
fp_rhs_arg.frac_prec);
|
|
FIX_PT_CHK_MATCH_MASK(
|
|
fp_lhs_arg.int_mask,
|
|
fp_rhs_arg.int_mask);
|
|
FIX_PT_CHK_MATCH_MASK(
|
|
fp_lhs_arg.frac_mask,
|
|
fp_rhs_arg.frac_mask);
|
|
|
|
if (fp_lhs_arg.int_part != fp_rhs_arg.int_part)
|
|
return 0;
|
|
|
|
if (fp_lhs_arg.frac_part != fp_rhs_arg.frac_part)
|
|
return 0;
|
|
|
|
return 1;
|
|
}
|
|
|
|
int fixed_point_to_int(
|
|
struct fixed_point fp_arg,
|
|
unsigned int *error)
|
|
{
|
|
int ret_int;
|
|
|
|
if (fp_arg.int_part & (1 << (fp_arg.int_prec - 1))) /* sign extend */
|
|
ret_int = 0 - ((int)fixed_point_negate(fp_arg, error).int_part);
|
|
else
|
|
ret_int = fp_arg.int_part;
|
|
|
|
return ret_int;
|
|
}
|
|
|
|
int fixed_point_ceil(
|
|
struct fixed_point fp_arg,
|
|
unsigned int *error)
|
|
{
|
|
int ret_int;
|
|
|
|
if (fp_arg.int_part & (1 << (fp_arg.int_prec - 1))) { /* negative */
|
|
struct fixed_point neg_fp = fixed_point_negate(fp_arg, error);
|
|
if (neg_fp.frac_part != 0)
|
|
ret_int = 0 - ((int)neg_fp.int_part) + 1;
|
|
else
|
|
ret_int = neg_fp.int_part;
|
|
} else {
|
|
if (fp_arg.frac_part != 0)
|
|
ret_int = fp_arg.int_part + 1;
|
|
else
|
|
ret_int = fp_arg.int_part;
|
|
}
|
|
|
|
return ret_int;
|
|
}
|
|
|
|
struct fixed_point fixed_point_min(
|
|
struct fixed_point fp_arg1,
|
|
struct fixed_point fp_arg2,
|
|
unsigned int *error)
|
|
{
|
|
if (fixed_point_lt(fp_arg1, fp_arg2, error))
|
|
return fp_arg1;
|
|
else
|
|
return fp_arg2;
|
|
}
|
|
|
|
struct fixed_point fixed_point_max(
|
|
struct fixed_point fp_arg1,
|
|
struct fixed_point fp_arg2,
|
|
unsigned int *error)
|
|
{
|
|
if (fixed_point_gt(fp_arg1, fp_arg2, error))
|
|
return fp_arg1;
|
|
else
|
|
return fp_arg2;
|
|
}
|