release/src-rt-6.x/shared/qmath.c

   1 /*
   2  * qmath functions used in arithmatic and DSP operations where
   3  * fractional operations, saturation support is needed.
   4  *
   5  * Copyright (C) 2009, Broadcom Corporation
   6  * All Rights Reserved.
   7  *
   8  * This is UNPUBLISHED PROPRIETARY SOURCE CODE of Broadcom Corporation;
   9  * the contents of this file may not be disclosed to third parties, copied
  10  * or duplicated in any form, in whole or in part, without the prior
  11  * written permission of Broadcom Corporation.
  12  *
  13  * $Id: qmath.c,v 1.3 2008/01/12 05:06:51 Exp $
  14  */
  15
  16 #include "qmath.h"
  17
  18 /*
  19 Description: This function saturate input 32 bit number into a 16 bit number.
  20 If input number is greater than 0x7fff then output is saturated to 0x7fff.
  21 else if input number is less than 0xffff8000 then output is saturated to 0xffff8000
  22 else output is same as input.
  23 */
  24 int16
  25 qm_sat32(int32 op)
  26 {
  27         int16 result;
  28         if (op > (int32)0x7fff) {
  29                 result = 0x7fff;
  30         }
  31         else if (op < (int32)0xffff8000) {
  32                 result = (int16)(0x8000);
  33         }
  34         else {
  35                 result = (int16)op;
  36         }
  37         return result;
  38 }
  39
  40 /*
  41 Description: This function multiply two input 16 bit numbers and return the 32 bit result.
  42 This multiplication is similar to compiler multiplication. This operation is defined if
  43 16 bit multiplication on the processor platform is cheaper than 32 bit multiplication (as
  44 the most of qmath functions can be replaced with processor intrinsic instructions).
  45 */
  46 int32
  47 qm_mul321616(int16 op1, int16 op2)
  48 {
  49         return ((int32)(op1)*(int32)(op2));
  50 }
  51
  52 /*
  53 Description: This function make 16 bit multiplication and return the result in 16 bits.
  54 To fit the result into 16 bits the 32 bit multiplication result is right
  55 shifted by 16 bits.
  56 */
  57 int16
  58 qm_mul16(int16 op1, int16 op2)
  59 {
  60         int32 result;
  61         result = ((int32)(op1)*(int32)(op2));
  62         return ((int16)(result >> 16));
  63 }
  64
  65 /*
  66 Description: This function multiply two 16 bit numbers and return the result in 32 bits.
  67 This function remove the extra sign bit created by the multiplication by leftshifting the
  68 32 bit multiplication result by 1 bit before returning the result. So the output is
  69 twice that of compiler multiplication. (i.e. qm_muls321616(2,3)=12).
  70 When both input 16 bit numbers are 0x8000, then the result is saturated to 0x7fffffff.
  71 */
  72 int32
  73 qm_muls321616(int16 op1, int16 op2)
  74 {
  75         int32 result;
  76         if (op1 == (int16)(0x8000) && op2 == (int16)(0x8000)) {
  77                 result = 0x7fffffff;
  78         }
  79         else {
  80                 result = ((int32)(op1)*(int32)(op2));
  81                 result = result << 1;
  82         }
  83         return result;
  84 }
  85
  86 /*
  87 Description: This function make 16 bit unsigned multiplication. To fit the output into
  88 16 bits the 32 bit multiplication result is right shifted by 16 bits.
  89 */
  90 uint16
  91 qm_mulu16(uint16 op1, uint16 op2)
  92 {
  93         return (uint16)(((uint32)op1 * (uint32)op2)>>16);
  94 }
  95
  96 /*
  97 Description: This function make 16 bit multiplication and return the result in 16 bits.
  98 To fit the multiplication result into 16 bits the multiplication result is right shifted by
  99 15 bits. Right shifting 15 bits instead of 16 bits is done to remove the extra sign bit formed
 100 due to the multiplication.
 101 When both the 16bit inputs are 0x8000 then the output is saturated to 0x7fffffff.
 102 */
 103 int16
 104 qm_muls16(int16 op1, int16 op2)
 105 {
 106         int32 result;
 107         if (op1 == (int16)0x8000 && op2 == (int16)0x8000) {
 108                 result = 0x7fffffff;
 109         }
 110         else {
 111                 result = ((int32)(op1)*(int32)(op2));
 112         }
 113         return ((int16)(result >> 15));
 114 }
 115
 116 /*
 117 Description: This function add two 32 bit numbers and return the 32bit result.
 118 If the result overflow 32 bits, the output will be saturated to 32bits.
 119 */
 120 int32
 121 qm_add32(int32 op1, int32 op2)
 122 {
 123         int32 result;
 124         result = op1 + op2;
 125         if (op1 < 0 && op2 < 0 && result > 0) {
 126                 result = 0x80000000;
 127         } else if (op1 > 0 && op2 > 0 && result < 0) {
 128                 result = 0x7fffffff;
 129         }
 130         return result;
 131 }
 132
 133 /*
 134 Description: This function add two 16 bit numbers and return the 16bit result.
 135 If the result overflow 16 bits, the output will be saturated to 16bits.
 136 */
 137 int16
 138 qm_add16(int16 op1, int16 op2)
 139 {
 140         int16 result;
 141         int32 temp = (int32)op1 + (int32)op2;
 142         if (temp > (int32)0x7fff) {
 143                 result = (int16)0x7fff;
 144         } else if (temp < (int32)0xffff8000) {
 145                 result = (int16)0xffff8000;
 146         } else {
 147                 result = (int16) temp;
 148         }
 149         return result;
 150 }
 151
 152 /*
 153 Description: This function make 16 bit subtraction and return the 16bit result.
 154 If the result overflow 16 bits, the output will be saturated to 16bits.
 155 */
 156 int16
 157 qm_sub16(int16 op1, int16 op2)
 158 {
 159         int16 result;
 160         int32 temp = (int32)op1 - (int32)op2;
 161         if (temp > (int32)0x7fff) {
 162                 result = (int16)0x7fff;
 163         } else if (temp < (int32)0xffff8000) {
 164                 result = (int16)0xffff8000;
 165         } else {
 166                 result = (int16) temp;
 167         }
 168         return result;
 169 }
 170
 171 /*
 172 Description: This function make 32 bit subtraction and return the 32bit result.
 173 If the result overflow 32 bits, the output will be saturated to 32bits.
 174 */
 175 int32
 176 qm_sub32(int32 op1, int32 op2)
 177 {
 178         int32 result;
 179         result = op1 - op2;
 180         if (op1 >= 0 && op2 < 0 && result < 0) {
 181                 result = 0x7fffffff;
 182         }
 183         else if (op1 < 0 && op2 > 0 && result > 0)      {
 184                 result = 0x80000000;
 185         }
 186         return result;
 187 }
 188
 189 /*
 190 Description: This function multiply input 16 bit numbers and accumulate the result
 191 into the input 32 bit number and return the 32 bit accumulated result.
 192 If the accumulation result in overflow, then the output will be saturated.
 193 */
 194 int32
 195 qm_mac321616(int32 acc, int16 op1, int16 op2)
 196 {
 197         int32 result;
 198         result = qm_add32(acc, qm_mul321616(op1, op2));
 199         return result;
 200 }
 201
 202 /*
 203 Description: This function make a 32 bit saturated left shift when the specified shift
 204 is +ve. This function will make a 32 bit right shift when the specified shift is -ve.
 205 This function return the result after shifting operation.
 206 */
 207 int32
 208 qm_shl32(int32 op, int shift)
 209 {
 210         int i;
 211         int32 result;
 212         result = op;
 213         if (shift > 31) shift = 31;
 214         else if (shift < -31) shift = -31;
 215         if (shift >= 0) {
 216                 for (i = 0; i < shift; i++) {
 217                         result = qm_add32(result, result);
 218                 }
 219         }
 220         else {
 221                 result = result >> (-shift);
 222         }
 223         return result;
 224 }
 225
 226 /*
 227 Description: This function make a 32 bit right shift when shift is +ve.
 228 This function make a 32 bit saturated left shift when shift is -ve. This function
 229 return the result of the shift operation.
 230 */
 231 int32
 232 qm_shr32(int32 op, int shift)
 233 {
 234         return qm_shl32(op, -shift);
 235 }
 236
 237 /*
 238 Description: This function make a 16 bit saturated left shift when the specified shift
 239 is +ve. This function will make a 16 bit right shift when the specified shift is -ve.
 240 This function return the result after shifting operation.
 241 */
 242 int16
 243 qm_shl16(int16 op, int shift)
 244 {
 245         int i;
 246         int16 result;
 247         result = op;
 248         if (shift > 15) shift = 15;
 249         else if (shift < -15) shift = -15;
 250         if (shift > 0) {
 251                 for (i = 0; i < shift; i++) {
 252                         result = qm_add16(result, result);
 253                 }
 254         }
 255         else {
 256                 result = result >> (-shift);
 257         }
 258         return result;
 259 }
 260
 261 /*
 262 Description: This function make a 16 bit right shift when shift is +ve.
 263 This function make a 16 bit saturated left shift when shift is -ve. This function
 264 return the result of the shift operation.
 265 */
 266 int16
 267 qm_shr16(int16 op, int shift)
 268 {
 269         return qm_shl16(op, -shift);
 270 }
 271
 272 /*
 273 Description: This function return the number of redundant sign bits in a 16 bit number.
 274 Example: qm_norm16(0x0080) = 7.
 275 */
 276 int16
 277 qm_norm16(int16 op)
 278 {
 279         uint16 u16extraSignBits;
 280         if (op == 0) {
 281                 return 15;
 282         }
 283         else {
 284                 u16extraSignBits = 0;
 285                 while ((op >> 15) == (op >> 14)) {
 286                         u16extraSignBits++;
 287                         op = op << 1;
 288                 }
 289         }
 290         return u16extraSignBits;
 291 }
 292
 293 /*
 294 Description: This function return the number of redundant sign bits in a 32 bit number.
 295 Example: qm_norm32(0x00000080) = 23
 296 */
 297 int16
 298 qm_norm32(int32 op)
 299 {
 300         uint16 u16extraSignBits;
 301         if (op == 0) {
 302                 return 31;
 303         }
 304         else {
 305                 u16extraSignBits = 0;
 306                 while ((op >> 31) == (op >> 30)) {
 307                         u16extraSignBits++;
 308                         op = op << 1;
 309                 }
 310         }
 311         return u16extraSignBits;
 312 }
 313
 314 /*
 315 Description: This function divide two 16 bit unsigned numbers.
 316 The numerator should be less than denominator. So the quotient is always less than 1.
 317 This function return the quotient in q.15 format.
 318 */
 319 int16
 320 qm_div_s(int16 num, int16 denom)
 321 {
 322         int16 var_out;
 323         int16 iteration;
 324         int32 L_num;
 325         int32 L_denom;
 326         L_num = (num) << 15;
 327         L_denom = (denom) << 15;
 328         for (iteration = 0; iteration < 15; iteration++) {
 329                 L_num <<= 1;
 330                 if (L_num >= L_denom) {
 331                         L_num = qm_sub32(L_num, L_denom);
 332                         L_num = qm_add32(L_num, 1);
 333                 }
 334         }
 335         var_out = (int16)(L_num & 0x7fff);
 336         return (var_out);
 337 }
 338
 339 /*
 340 Description: This function compute the absolute value of a 16 bit number.
 341 */
 342 int16
 343 qm_abs16(int16 op)
 344 {
 345         if (op < 0) {
 346                 if (op == (int16)0xffff8000) {
 347                         return 0x7fff;
 348                 } else {
 349                         return -op;
 350                 }
 351         }
 352         else {
 353                 return op;
 354         }
 355 }
 356
 357 /*
 358 Description: This function divide two 16 bit numbers.
 359 The quotient is returned through return value.
 360 The qformat of the quotient is returned through the pointer (qQuotient) passed
 361 to this function. The qformat of quotient is adjusted appropriately such that
 362 the quotient occupies all 16 bits.
 363 */
 364 int16
 365 qm_div16(int16 num, int16 denom, int16 *qQuotient)
 366 {
 367         int16 sign;
 368         int16 nNum, nDenom;
 369         sign = num ^ denom;
 370         num = qm_abs16(num);
 371         denom = qm_abs16(denom);
 372         nNum = qm_norm16(num);
 373         nDenom = qm_norm16(denom);
 374         num = qm_shl16(num, nNum-1);
 375         denom = qm_shl16(denom, nDenom);
 376         *qQuotient = nNum-1-nDenom+15;
 377         if (sign >= 0)  {
 378                 return qm_div_s(num, denom);
 379         }
 380         else {
 381                 return -qm_div_s(num, denom);
 382         }
 383 }
 384
 385 /*
 386 Description: This function compute absolute value of a 32 bit number.
 387 */
 388 int32
 389 qm_abs32(int32 op)
 390 {
 391         if (op < 0) {
 392                 if (op == (int32)0x80000000) {
 393                         return 0x7fffffff;
 394                 } else {
 395                         return -op;
 396                 }
 397         }
 398         else {
 399                 return op;
 400         }
 401 }
 402
 403 /*
 404 Description: This function divide two 32 bit numbers. The division is performed
 405 by considering only important 16 bits in 32 bit numbers.
 406 The quotient is returned through return value.
 407 The qformat of the quotient is returned through the pointer (qquotient) passed
 408 to this function. The qformat of quotient is adjusted appropriately such that
 409 the quotient occupies all 16 bits.
 410 */
 411 int16
 412 qm_div163232(int32 num, int32 denom, int16 *qquotient)
 413 {
 414         int32 sign;
 415         int16 nNum, nDenom;
 416         sign = num ^ denom;
 417         num = qm_abs32(num);
 418         denom = qm_abs32(denom);
 419         nNum = qm_norm32(num);
 420         nDenom = qm_norm32(denom);
 421         num = qm_shl32(num, nNum - 1);
 422         denom = qm_shl32(denom, nDenom);
 423         *qquotient = nNum - 1 - nDenom + 15;
 424         if (sign >= 0)  {
 425                 return qm_div_s((int16)(num >> 16), (int16)(denom >> 16));
 426         }
 427         else {
 428                 return -qm_div_s((int16)(num >> 16), (int16)(denom >> 16));
 429         }
 430 }
 431
 432 /*
 433 Description: This function multiply a 32 bit number with a 16 bit number.
 434 The multiplicaton result is right shifted by 16 bits to fit the result
 435 into 32 bit output.
 436 */
 437 int32
 438 qm_mul323216(int32 op1, int16 op2)
 439 {
 440         int16 hi;
 441         uint16 lo;
 442         int32 result;
 443         hi = op1 >> 16;
 444         lo = (int16)(op1 & 0xffff);
 445         result = qm_mul321616(hi, op2);
 446         result = result + (qm_mulsu321616(op2, lo) >> 16);
 447         return result;
 448 }
 449
 450 /*
 451 Description: This function multiply signed 16 bit number with unsigned 16 bit number and return
 452 the result in 32 bits.
 453 */
 454 int32
 455 qm_mulsu321616(int16 op1, uint16 op2)
 456 {
 457         return (int32)(op1) * op2;
 458 }
 459
 460 /*
 461 Description: This function multiply 32 bit number with 16 bit number. The multiplication result is
 462 right shifted by 15 bits to fit the result into 32 bits. Right shifting by only 15 bits instead of
 463 16 bits is done to remove the extra sign bit formed by multiplication from the return value.
 464 When the input numbers are 0x80000000, 0x8000 the return value is saturated to 0x7fffffff.
 465 */
 466 int32
 467 qm_muls323216(int32 op1, int16 op2)
 468 {
 469         int16 hi;
 470         uint16 lo;
 471         int32 result;
 472         hi = op1 >> 16;
 473         lo = (int16)(op1 & 0xffff);
 474         result = qm_muls321616(hi, op2);
 475         result = qm_add32(result, (qm_mulsu321616(op2, lo) >> 15));
 476         return result;
 477 }
 478
 479 /*
 480 Description: This function multiply two 32 bit numbers. The multiplication result is right
 481 shifted by 32 bits to fit the multiplication result into 32 bits. The right shifted
 482 multiplication result is returned as output.
 483 */
 484 int32
 485 qm_mul32(int32 a, int32 b)
 486 {
 487         int16 hi1, hi2;
 488         uint16 lo1, lo2;
 489         int32 result;
 490         hi1 = a >> 16;
 491         hi2 = b >> 16;
 492         lo1 = (uint16)(a & 0xffff);
 493         lo2 = (uint16)(b & 0xffff);
 494         result = qm_mul321616(hi1, hi2);
 495         result = result + (qm_mulsu321616(hi1, lo2) >> 16);
 496         result = result + (qm_mulsu321616(hi2, lo1) >> 16);
 497         return result;
 498 }
 499
 500 /*
 501 Description: This function multiply two 32 bit numbers. The multiplication result is
 502 right shifted by 31 bits to fit the multiplication result into 32 bits. The right
 503 shifted multiplication result is returned as output. Right shifting by only 31 bits
 504 instead of 32 bits is done to remove the extra sign bit formed by multiplication.
 505 When the input numbers are 0x80000000, 0x80000000 the return value is saturated to
 506 0x7fffffff.
 507 */
 508 int32
 509 qm_muls32(int32 a, int32 b)
 510 {
 511         int16 hi1, hi2;
 512         uint16 lo1, lo2;
 513         int32 result;
 514         hi1 = a >> 16;
 515         hi2 = b >> 16;
 516         lo1 = (uint16)(a & 0xffff);
 517         lo2 = (uint16)(b & 0xffff);
 518         result = qm_muls321616(hi1, hi2);
 519         result = qm_add32(result, (qm_mulsu321616(hi1, lo2) >> 15));
 520         result = qm_add32(result, (qm_mulsu321616(hi2, lo1) >> 15));
 521         result = qm_add32(result, (qm_mulu16(lo1, lo2) >> 15));
 522         return result;
 523 }
 524
 525 /* This table is log2(1+(i/32)) where i=[0:1:31], in q.15 format */
 526 int16 log_table[] = {
 527 0,
 528 1455,
 529 2866,
 530 4236,
 531 5568,
 532 6863,
 533 8124,
 534 9352,
 535 10549,
 536 11716,
 537 12855,
 538 13968,
 539 15055,
 540 16117,
 541 17156,
 542 18173,
 543 19168,
 544 20143,
 545 21098,
 546 22034,
 547 22952,
 548 23852,
 549 24736,
 550 25604,
 551 26455,
 552 27292,
 553 28114,
 554 28922,
 555 29717,
 556 30498,
 557 31267,
 558 32024
 559 };
 560 #define LOG_TABLE_SIZE 32               /* log_table size */
 561 #define LOG2_LOG_TABLE_SIZE 5   /* log2(log_table size) */
 562 #define Q_LOG_TABLE 15                  /* qformat of log_table */
 563 #define LOG10_2         19728           /* log10(2) in q.16 */
 564
 565 /*
 566 Description:
 567 This routine takes the input number N and its q format qN and compute
 568 the log10(N). This routine first normalizes the input no N.     Then N is in mag*(2^x) format.
 569 mag is any number in the range 2^30-(2^31 - 1). Then log2(mag * 2^x) = log2(mag) + x is computed.
 570 From that log10(mag * 2^x) = log2(mag * 2^x) * log10(2) is computed.
 571 This routine looks the log2 value in the table considering LOG2_LOG_TABLE_SIZE+1 MSBs.
 572 As the MSB is always 1, only next LOG2_OF_LOG_TABLE_SIZE MSBs are used for table lookup.
 573 Next 16 MSBs are used for interpolation.
 574 Inputs:
 575 N - number to which log10 has to be found.
 576 qN - q format of N
 577 log10N - address where log10(N) will be written.
 578 qLog10N - address where log10N qformat will be written.
 579 Note/Problem:
 580 For accurate results input should be in normalized or near normalized form.
 581 */
 582 void
 583 qm_log10(int32 N, int16 qN, int16 *log10N, int16 *qLog10N)
 584 {
 585         int16 s16norm, s16tableIndex, s16errorApproximation;
 586         uint16 u16offset;
 587         int32 s32log;
 588
 589         /* Logerithm of negative values is undefined.
 590         * assert N is greater than 0.
 591         */
 592         /* ASSERT(N > 0); */
 593
 594         /* normalize the N. */
 595         s16norm = qm_norm32(N);
 596         N = N << s16norm;
 597
 598         /* The qformat of N after normalization.
 599         * -30 is added to treat the no as between 1.0 to 2.0
 600         * i.e. after adding the -30 to the qformat the decimal point will be
 601         * just rigtht of the MSB. (i.e. after sign bit and 1st MSB). i.e.
 602         * at the right side of 30th bit.
 603         */
 604         qN = qN + s16norm - 30;
 605
 606         /* take the table index as the LOG2_OF_LOG_TABLE_SIZE bits right of the MSB */
 607         s16tableIndex = (int16)(N >> (32-(2+LOG2_LOG_TABLE_SIZE)));
 608
 609         /* remove the MSB. the MSB is always 1 after normalization. */
 610         s16tableIndex = s16tableIndex & (int16)((1<<LOG2_LOG_TABLE_SIZE)-1);
 611
 612         /* remove the (1+LOG2_OF_LOG_TABLE_SIZE) MSBs in the N. */
 613         N = N & ((1<<(32-(2+LOG2_LOG_TABLE_SIZE))) - 1);
 614
 615         /* take the offset as the 16 MSBS after table index.
 616         */
 617         u16offset = (uint16)(N >> (32-(2+LOG2_LOG_TABLE_SIZE+16)));
 618
 619         /* look the log value in the table. */
 620         s32log = log_table[s16tableIndex];              /* q.15 format */
 621
 622         /* interpolate using the offset. */
 623         s16errorApproximation = (int16)qm_mulu16(u16offset, \
 624                 (uint16)(log_table[s16tableIndex+1]-log_table[s16tableIndex])); /* q.15 */
 625
 626         s32log = qm_add16((int16)s32log, s16errorApproximation);        /* q.15 format */
 627
 628         /* adjust for the qformat of the N as
 629         * log2(mag * 2^x) = log2(mag) + x
 630         */
 631         s32log = qm_add32(s32log, ((int32)-qN)<<15);            /* q.15 format */
 632
 633         /* normalize the result. */
 634         s16norm = qm_norm32(s32log);
 635
 636         /* bring all the important bits into lower 16 bits */
 637         s32log = qm_shl32(s32log, s16norm-16);          /* q.15+s16norm-16 format */
 638
 639         /* compute the log10(N) by multiplying log2(N) with log10(2).
 640         * as log10(mag * 2^x) = log2(mag * 2^x) * log10(2)
 641         * log10N in q.15+s16norm-16+1 (LOG10_2 is in q.16)
 642         */
 643         *log10N = qm_muls16((int16)s32log, (int16)LOG10_2);
 644
 645         /* write the q format of the result. */
 646         *qLog10N = 15 + s16norm - 16 + 1;
 647
 648         return;
 649 }
 650
 651 /*
 652 Description:
 653 This routine compute 1/N.
 654 This routine reformates the given no N as N * 2^qN where N is in between 0.5 and 1.0
 655 in q.15 format in 16 bits. So the problem now boils down to finding the inverse of a
 656 q.15 no in 16 bits which is in the range of 0.5 to 1.0. The output is always between
 657 2.0 to 1. So the output is 2.0 to 1.0 in q.30 format. Once the final output format is found
 658 by taking the qN into account. Inverse is found with newton rapson method. Initially
 659 inverse (x) is guessed as 1/0.75 (with appropriate sign). The new guess is calculated
 660 using the formula x' = 2*x - N*x*x. After 4 or 5 iterations the inverse is very close to
 661 inverse of N.
 662 Inputs:
 663 N - number to which 1/N has to be found.
 664 qn - q format of N.
 665 sqrtN - address where 1/N has to be written.
 666 qsqrtN - address where q format of 1/N has to be written.
 667 */
 668 #define qx 29
 669 void
 670 qm_1byN(int32 N, int16 qN, int32 *result, int16 *qResult)
 671 {
 672         int16 normN;
 673         int32 s32firstTerm, s32secondTerm, x;
 674         int i;
 675
 676         normN = qm_norm32(N);
 677
 678         /* limit N to least significant 16 bits. 15th bit is the sign bit. */
 679         N = qm_shl32(N, normN-16);
 680         qN = qN + normN - 16 - 15;
 681         /* -15 is added to treat N as 16 bit q.15 number in the range from 0.5 to 1 */
 682
 683         /* Take the initial guess as 1/0.75 in qx format with appropriate sign. */
 684         if (N >= 0)
 685         {
 686                 x = (int32)((1/0.75)*(1<<qx));
 687                 /* input no is in the range 0.5 to 1. So 1/0.75 is taken as initial guess. */
 688         }
 689         else
 690         {
 691                 x = (int32)((1/-0.75)*(1<<qx));
 692                 /* input no is in the range -0.5 to -1. So 1/-0.75 is taken as initial guess. */
 693         }
 694
 695         /* iterate the equation x = 2*x - N*x*x for 4 times. */
 696         for (i = 0; i < 4; i++)
 697         {
 698                 s32firstTerm = qm_shl32(x, 1); /* s32firstTerm = 2*x in q.29 */
 699                 s32secondTerm = qm_muls321616((int16)(s32firstTerm>>16), (int16)(s32firstTerm>>16));
 700                 /* s32secondTerm = x*x in q.(29+1-16)*2+1 */
 701                 s32secondTerm = qm_muls321616((int16)(s32secondTerm>>16), (int16)N);
 702                 /* s32secondTerm = N*x*x in q.((29+1-16)*2+1)-16+15+1 i.e. in q.29 */
 703                 x = qm_sub32(s32firstTerm, s32secondTerm);
 704                 /* can be added directly as both are in q.29 */
 705         }
 706
 707         /* Bring the x to q.30 format. */
 708         *result = qm_shl32(x, 1);
 709         /* giving the output in q.30 format for q.15 input in 16 bits. */
 710
 711         /* compute the final q format of the result. */
 712         *qResult = -qN + 30;    /* adjusting the q format of actual output */
 713
 714         return;
 715 }
 716 #undef qx