sys/dev/drm/amd/display/dc/basics/fixpt31_32.c

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