diff options
Diffstat (limited to 'keymaps/onekey/f411_qfplib/qfplib/qfplib.s')
-rw-r--r-- | keymaps/onekey/f411_qfplib/qfplib/qfplib.s | 832 |
1 files changed, 832 insertions, 0 deletions
diff --git a/keymaps/onekey/f411_qfplib/qfplib/qfplib.s b/keymaps/onekey/f411_qfplib/qfplib/qfplib.s new file mode 100644 index 0000000..e035472 --- /dev/null +++ b/keymaps/onekey/f411_qfplib/qfplib/qfplib.s @@ -0,0 +1,832 @@ +// Copyright 2015 Mark Owen ([email protected]) +// SPDX-License-Identifier: GPL-2.0-or-later + +.ifndef include_faster +.equ include_faster,1 +.endif + +.ifndef include_conversions +.equ include_conversions,1 +.endif + +.ifndef include_scientific +.equ include_scientific,1 +.endif + +.if include_scientific +.equ include_conversions,1 +.endif + +.syntax unified +.cpu cortex-m0 +.thumb + +.global qfp_fadd +.global qfp_fsub +.global qfp_fmul +.global qfp_fdiv +.global qfp_fcmp +.if include_conversions +.global qfp_float2int +.global qfp_float2fix +.global qfp_float2uint +.global qfp_float2ufix +.global qfp_int2float +.global qfp_fix2float +.global qfp_uint2float +.global qfp_ufix2float +.endif +.if include_scientific +.global qfp_fcos +.global qfp_fsin +.global qfp_ftan +.global qfp_fatan2 +.global qfp_fexp +.global qfp_fln +.global qfp_fsqrt +.endif + +.if include_faster +.global qfp_fdiv_fast +.global qfp_fsqrt_fast +.endif + +// exchange r0<->r1, r2<->r3 +xchxy: + push {r0,r2,r14} + mov r0,r1 + mov r2,r3 + pop {r1,r3,r15} + +// IEEE single precision floats in r0,r1-> mantissae in r1,r0 exponents in r3,r2 *respectively* +// trashes r4 +unpackxy: + push {r14} + bl unpackx + bl xchxy + pop {r4} + mov r14,r4 + +// IEEE single in r0-> signed (two's complemennt) mantissa in r0 9Q23 (24 significant bits), signed exponent (bias removed) in r2 +// trashes r4; zero, denormal -> mantissa=+/-1, exponent=-380; Inf, NaN -> mantissa=+/-1, exponent=+640 +unpackx: + lsrs r2,r0,#23 // save exponent and sign + lsls r0,#9 // extract mantissa + lsrs r0,#9 + movs r4,#1 + lsls r4,#23 + orrs r0,r4 // reinstate implied leading 1 + cmp r2,#255 // test sign bit + uxtb r2,r2 // clear it + bls 1f // branch on positive + rsbs r0,#0 // negate mantissa +1: + subs r2,#1 + cmp r2,#254 // zero/denormal/Inf/NaN? + bhs 2f + subs r2,#126 // remove exponent bias: can now be -126..+127 + bx r14 + +2: // here with special-case values + cmp r0,#0 + mov r0,r4 // set mantissa to +1 + bpl 3f + rsbs r0,#0 // zero/denormal/Inf/NaN: mantissa=+/-1 +3: + subs r2,#126 // zero/denormal: exponent -> -127; Inf, NaN: exponent -> 128 + lsls r2,#2 // zero/denormal: exponent -> -508; Inf, NaN: exponent -> 512 + adds r2,#128 // zero/denormal: exponent -> -380; Inf, NaN: exponent -> 640 + bx r14 + +// normalise and pack signed mantissa in r0 nominally 3Q29, signed exponent in r2-> IEEE single in r0 +// trashes r4, preserves r1,r3 +// r5: "sticky bits", must be zero iff all result bits below r0 are zero for correct rounding +packx: + lsrs r4,r0,#31 // save sign bit + lsls r4,r4,#31 // sign now in b31 + bpl 2f // skip if positive + cmp r5,#0 + beq 11f + adds r0,#1 // fiddle carry in to following rsb if sticky bits are non-zero +11: + rsbs r0,#0 // can now treat r0 as unsigned +packx0: + bmi 3f // catch r0=0x80000000 case +2: + subs r2,#1 // normalisation loop + adds r0,r0 + beq 1f // zero? special case + bpl 2b // normalise so leading "1" in bit 31 +3: + adds r2,#129 // (mis-)offset exponent + bne 12f // special case: highest denormal can round to lowest normal + adds r0,#0x80 // in special case, need to add 256 to r0 for rounding + bcs 4f // tripped carry? then have leading 1 in C as required +12: + adds r0,#0x80 // rounding + bcs 4f // tripped carry? then have leading 1 in C as required (and result is even so can ignore sticky bits) + cmp r5,#0 + beq 7f // sticky bits zero? +8: + lsls r0,#1 // remove leading 1 +9: + subs r2,#1 // compensate exponent on this path +4: + cmp r2,#254 + bge 5f // overflow? + adds r2,#1 // correct exponent offset + ble 10f // denormal/underflow? + lsrs r0,#9 // align mantissa + lsls r2,#23 // align exponent + orrs r0,r2 // assemble exponent and mantissa +6: + orrs r0,r4 // apply sign +1: + bx r14 + +5: + movs r0,#0xff // create infinity + lsls r0,#23 + b 6b + +10: + movs r0,#0 // create zero + bx r14 + +7: // sticky bit rounding case + lsls r5,r0,#24 // check bottom 8 bits of r0 + bne 8b // in rounding-tie case? + lsrs r0,#9 // ensure even result + lsls r0,#10 + b 9b + +// unpack two arguments (r0,r1) and shift one down to have common exponent, returned in r2; note that arguments are exchanged +// sticky bits shifted off bottom of smaller argument in r5 +// following code is unnecessarily general for fadd, but is shared with atan2 +unpackxyalign: + push {r14} + bl unpackxy + lsls r0,r0,#6 // Q29 + lsls r1,r1,#6 // Q29 + subs r4,r2,r3 // calculate shift + bge 1f // x>=y? + mov r2,r3 // no: take common exponent from y + mov r5,r0 // potential sticky bits from x + rsbs r4,#0 // make shift positive + asrs r0,r4 + cmp r4,#32 + blo 2f + movs r0,#0 // large shift, so all bits are sticky and result is zero + pop {r15} +1: + mov r5,r1 // common exponent from x; potential sticky bits from y + asrs r1,r4 + cmp r4,#32 + blo 2f + movs r1,#0 // large shift, so all bits are sticky and result is zero + pop {r15} +2: + rsbs r4,#0 + adds r4,#32 + lsls r5,r4 // extract sticky bits + pop {r15} + +.thumb_func +qfp_fsub: + movs r2,#1 // subtract: flip sign bit of second argument and fall through to fadd + lsls r2,#31 + eors r1,r2 +.thumb_func +qfp_fadd: + push {r4,r5,r14} + bl unpackxyalign + adds r0,r1 // do addition + bne 2f // not in Inf-Inf case? + cmp r2,#200 + blt 2f + movs r0,#1 + lsls r0,#29 // for Inf-Inf, set mantissa to +1 to prevent zero result +2: +packret: // common return point: "pack and return" + bl packx + pop {r4,r5,r15} + +// signed multiply r0 1Q23 by r1 4Q23, result in r0 7Q25, sticky bits in r5 +// trashes r3,r4 +mul0: + uxth r3,r0 // Q23 + asrs r4,r1,#16 // Q7 + muls r3,r4 // L*H, Q30 signed + asrs r4,r0,#16 // Q7 + uxth r5,r1 // Q23 + muls r4,r5 // H*L, Q30 signed + adds r3,r4 // sum of middle partial products + uxth r4,r0 + muls r4,r5 // L*L, Q46 unsigned + lsls r5,r4,#16 // initialise sticky bits from low half of low partial product + lsrs r4,#16 // Q25 + adds r3,r4 // add high half of low partial product to sum of middle partial products + // (cannot generate carry by limits on input arguments) + asrs r0,#16 // Q7 + asrs r1,#16 // Q7 + muls r0,r1 // H*H, Q14 signed + lsls r0,#11 // high partial product Q25 + lsls r1,r3,#27 // sticky + orrs r5,r1 // collect further sticky bits + asrs r1,r3,#5 // middle partial products Q25 + adds r0,r1 // final result + bx r14 + +.thumb_func +qfp_fcmp: + movs r2,#1 // initialise result + lsls r3,r2,#31 // r3=0x80000000 + tst r0,r3 // check sign of first argument + beq 1f + subs r0,r3,r0 // convert to 2's complement form for direct comparison +1: + tst r1,r3 // repeat for second argument + beq 2f + subs r1,r3,r1 +2: + subs r0,r1 // perform comparison + beq 4f // equal? return 0 + bgt 3f // r0>r1? return +1 + rsbs r2,#0 // r0<r1: return -1 +3: + mov r0,r2 +4: + bx r14 + +.thumb_func +qfp_fmul: + push {r4,r5,r14} + bl unpackxy + add r2,r3 // sum exponents + adds r2,#4 // adjust exponent for pack which expects Q29 +fmul0: + bl mul0 + b packret + +.thumb_func +qfp_fdiv: + push {r4,r5,r14} +fdiv_n: + bl unpackxy + movs r5,#1 // result cannot fall exactly half-way between two representable numbers (exercise for reader - note that + // we do not handle denormals) so there will always be sticky bits + cmp r0,#0 // check divisor sign + bpl 1f + rsbs r0,#0 // ensure divisor positive + rsbs r1,#0 // preserve sign of result +1: + movs r4,#0 + cmp r1,#0 // check sign of dividend + bpl 2f + rsbs r1,#0 // result will be negative + mvns r4,r4 // save sign as 0x00000000 or 0xffffffff +2: // now do unsigned division on unpacked values {r1,r3}/{r0,r2} + cmp r3,#200 // dividend is an infinity? return it + bge 3f + rsbs r2,#0 + cmp r2,#200 // divisor is zero? return infinity + bge 3f + adds r2,r3 // difference of exponents + movs r3,#0x40 // termination marker (calculate enough bits to do rounding correctly) +2: // division loop + subs r1,r0 // trial subtraction + bcs 1f + add r1,r0 // restore if failed +1: + adcs r3,r3 // shift in result bit + add r1,r1 // shift up dividend + bcc 2b // loop until marker appears in carry + lsls r0,r3,#4 // align for packing +4: + eors r0,r4 // apply sign + b packret +infret: + movs r4,#0 +3: + mov r0,r1 + movs r2,#255 // return infinity + b 4b + +.if include_faster + +// The fast divide routine uses an initial approximation to the reciprocal of the divisor based on the top four bits of the mantissa +// followed by three Newton-Raphson iterations, resulting in about 27 bits of accuracy. This reciprocal is then multiplied by +// the dividend. +// The (fixed-point) reciprocal calculation is carefully implemented to preserve the necessary accuracy throughout. In places +// the implied binary point is not within the stored value. For example where "Q47" is shown below it means that the least +// significant bit of the value has significance 2^-47 and hence the most significant bit has significance 2^-16. In these +// cases the value is known to be very close to an integer (usually 1) and so the bits of greater significance do not need +// to be stored. +// The reciprocal calculation has been tested for all possible input mantissa values. +.thumb_func +qfp_fdiv_fast: + push {r4,r5,r14} +fdiv_fast_n: + bl unpackxy + cmp r0,#0 + bpl 1f + rsbs r0,#0 + rsbs r1,#0 // ensure divisor positive +1: + cmp r3,#200 + bge infret // dividend is an infinity? return it + rsbs r2,#0 + cmp r2,#200 // divisor is zero? + bge infret // return infinity + adds r2,r3 // difference of exponents + adr r4,rcpapp-8 // the first 8 elements of the table are never accessed because of the mantissa's leading 1 + lsrs r3,r0,#20 // y Q23; y>>20 Q7 + ldrb r4,[r4,r3] // m=rcpapp[(y>>20)&7]; // Q8, .5<m<1 + + lsls r3,r4,#2 // m<<2 // Q10 first Newton-Raphson iteration + muls r3,r0 // s=y*(m<<2); // Q33 + lsls r4,#8 // m<<=8; // Q16 + asrs r3,#21 // s>>=21; // Q12 + muls r3,r4 // s*=m; // Q28 + asrs r3,#12 // s>>=12; // Q16 + subs r4,r3 // m=m-s; // Q16 + + mov r3,r4 // s=y*m // Q39 second Newton-Raphson iteration + muls r4,r0 // ... + asrs r4,#16 // s>>=16; // Q23 + muls r4,r3 // s*=m; // Q39 + lsls r3,#8 // m<<=8; // Q24 + asrs r4,#15 // s>>=15; // Q24 + subs r3,r4 // m=m-s; // Q24 + + lsls r4,r3,#7 // \/ s=y*m; // Q47 third Newton-Raphson iteration + muls r3,r0 // /\ m<<=7; // Q31 + asrs r3,#15 // s>>=15; // Q32 + lsrs r0,r4,#16 // s*=(m>>16); // Q47 + muls r3,r0 // ... + asrs r3,#16 // s>>=16; // Q31 + subs r0,r4,r3 // m=m-s; // Q31 +div0: + adds r0,#7 // rounding; reduce systematic error + lsrs r0,#4 // Q27 + b fmul0 // drop into multiplication code to calculate result + +// The fast square root routine uses an initial approximation to the reciprocal of the square root of the argument based +// on the top four bits of the mantissa (possibly shifted one place to make the exponent even). It then performs three +// Newton-Raphson iterations, resulting in about 28-29 bits of accuracy. This reciprocal is then multiplied by +// the original argument to produce the result. +// Again, the fixed-point calculation is carefully implemented to preserve accuracy, and similar comments to those +// made above on the fast division routine apply. +// The reciprocal square root calculation has been tested for all possible (possibly shifted) input mantissa values. +.thumb_func +qfp_fsqrt_fast: + push {r4,r5,r14} + bl unpackx + movs r1,r0 + bmi infret // negative? return -Inf + asrs r0,r2,#1 // check LSB of exponent + bcc 1f + lsls r1,#1 // was odd: double mantissa; mantissa y now 1..4 Q23 +1: + adds r2,#4 // correction for packing + adr r4,rsqrtapp-4// first four table entries are never accessed because of the mantissa's leading 1 + lsrs r3,r1,#21 // y>>21 Q2 + ldrb r4,[r4,r3] // initial approximation to reciprocal square root m Q8 + + lsrs r0,r1,#7 // y>>7 // Q16 first Newton-Raphson iteration + muls r0,r4 // m*y + muls r0,r4 // s=m*y*y // Q32 + asrs r0,#12 // s>>12 + muls r0,r4 // m*s // Q28 + asrs r0,#13 // m*s // Q15 + lsls r4,#8 // m // Q16 + subs r4,r0 // m=(m<<8)-(s>>13) // Q16-Q15/2 -> Q16 + + mov r0,r4 // // second Newton-Raphson iteration + muls r0,r0 // u=m*m // Q32 + lsrs r0,#16 // u>>16 // Q16 + lsrs r3,r1,#7 // y>>7 // Q16 + muls r0,r3 // s=u*(y>>7) // Q32 + asrs r0,#12 // s>>12 // Q20 + muls r0,r4 // s*m // Q36 + asrs r0,#21 // s*m // Q15 + subs r4,r0 // m=m-s // Q16-Q15/2 + + mov r0,r4 // // third Newton-Raphson iteration + muls r0,r0 // u=m*m // Q32 + lsrs r3,r0,#12 // now multiply u and y in two parts: u>>12 + muls r3,r1 // first partial product (u>>12)*y Q43 + lsls r0,#20 + lsrs r0,#20 // u&0xfff + lsrs r5,r1,#12 // y>>12 + muls r0,r5 // second partial product (u&0xfff)*(y>>12) Q43 + add r0,r3 // s=u*y // Q43 + asrs r0,#15 // s>>15 // Q28 + muls r0,r4 // (s>>15)*m // Q44 + lsls r4,#13 // m<<13 // Q29 + asrs r0,#16 // s>>16 // Q28 + subs r0,r4,r0 // // Q29-Q28/2 + + asrs r2,#1 // halve exponent + bcc div0 // was y shifted? + lsrs r0,#1 + lsls r1,#1 // shift y back + b div0 // round and complete with multiplication + +.align 2 + +// round(2^15./[136:16:248]) +rcpapp: +.byte 0xf1,0xd8,0xc3,0xb2, 0xa4,0x98,0x8d,0x84 + +// round(sqrt(2^22./[72:16:248])) +rsqrtapp: +.byte 0xf1,0xda,0xc9,0xbb, 0xb0,0xa6,0x9e,0x97, 0x91,0x8b,0x86,0x82 + +.endif + +.if include_conversions + +// convert float to signed int, rounding towards -Inf, clamping +.thumb_func +qfp_float2int: + movs r1,#0 // fall through + +// convert float in r0 to signed fixed point in r0, clamping +.thumb_func +qfp_float2fix: + push {r4,r14} + bl unpackx + add r2,r1 // incorporate binary point position into exponent + subs r2,#23 // r2 is now amount of left shift required + blt 1f // requires right shift? + cmp r2,#7 // overflow? + ble 4f +3: // overflow + asrs r1,r0,#31 // +ve:0 -ve:0xffffffff + mvns r1,r1 // +ve:0xffffffff -ve:0 + movs r0,#1 + lsls r0,#31 +5: + eors r0,r1 // +ve:0x7fffffff -ve:0x80000000 (unsigned path: 0xffffffff) + pop {r4,r15} +1: + rsbs r2,#0 // right shift for r0, >0 + cmp r2,#32 + blt 2f // more than 32 bits of right shift? + movs r2,#32 +2: + asrs r0,r0,r2 + pop {r4,r15} + +// unsigned version +.thumb_func +qfp_float2uint: + movs r1,#0 // fall through +.thumb_func +qfp_float2ufix: + push {r4,r14} + bl unpackx + add r2,r1 // incorporate binary point position into exponent + movs r1,r0 + bmi 5b // negative? return zero + subs r2,#23 // r2 is now amount of left shift required + blt 1b // requires right shift? + mvns r1,r0 // ready to return 0xffffffff + cmp r2,#8 // overflow? + bgt 5b +4: + lsls r0,r0,r2 // result fits, left shifted + pop {r4,r15} + +// convert signed int to float, rounding +.thumb_func +qfp_int2float: + movs r1,#0 // fall through + +// convert signed fix to float, rounding; number of r0 bits after point in r1 +.thumb_func +qfp_fix2float: + push {r4,r5,r14} +1: + movs r2,#29 + subs r2,r1 // fix exponent +packretns: // pack and return, sticky bits=0 + movs r5,#0 + b packret + +// unsigned version +.thumb_func +qfp_uint2float: + movs r1,#0 // fall through +.thumb_func +qfp_ufix2float: + push {r4,r5,r14} + cmp r0,#0 + bge 1b // treat <2^31 as signed + movs r2,#30 + subs r2,r1 // fix exponent + lsls r5,r0,#31 // one sticky bit + lsrs r0,#1 + b packret + +.endif + +.if include_scientific + +// All the scientific functions are implemented using the CORDIC algorithm. For notation, +// details not explained in the comments below, and a good overall survey see +// "50 Years of CORDIC: Algorithms, Architectures, and Applications" by Meher et al., +// IEEE Transactions on Circuits and Systems Part I, Volume 56 Issue 9. + +// Register use: +// r0: x +// r1: y +// r2: z/omega +// r3: coefficient pointer +// r4,r12: m +// r5: i (shift) + +cordic_start: // initialisation + movs r5,#0 // initial shift=0 + mov r12,r4 + b 5f + +cordic_vstep: // one step of algorithm in vector mode + cmp r1,#0 // check sign of y + bgt 4f + b 1f +cordic_rstep: // one step of algorithm in rotation mode + cmp r2,#0 // check sign of angle + bge 1f +4: + subs r1,r6 // negative rotation: y=y-(x>>i) + rsbs r7,#0 + adds r2,r4 // accumulate angle + b 2f +1: + adds r1,r6 // positive rotation: y=y+(x>>i) + subs r2,r4 // accumulate angle +2: + mov r4,r12 + muls r7,r4 // apply sign from m + subs r0,r7 // finish rotation: x=x{+/-}(y>>i) +5: + ldmia r3!,{r4} // fetch next angle from table and bump pointer + lsrs r4,#1 // repeated angle? + bcs 3f + adds r5,#1 // adjust shift if not +3: + mov r6,r0 + asrs r6,r5 // x>>i + mov r7,r1 + asrs r7,r5 // y>>i + lsrs r4,#1 // shift end flag into carry + bx r14 + +// CORDIC rotation mode +cordic_rot: + push {r6,r7,r14} + bl cordic_start // initialise +1: + bl cordic_rstep + bcc 1b // step until table finished + asrs r6,r0,#14 // remaining small rotations can be linearised: see IV.B of paper referenced above + asrs r7,r1,#14 + asrs r2,#3 + muls r6,r2 // all remaining CORDIC steps in a multiplication + muls r7,r2 + mov r4,r12 + muls r7,r4 + asrs r6,#12 + asrs r7,#12 + subs r0,r7 // x=x{+/-}(yz>>k) + adds r1,r6 // y=y+(xz>>k) +cordic_exit: + pop {r6,r7,r15} + +// CORDIC vector mode +cordic_vec: + push {r6,r7,r14} + bl cordic_start // initialise +1: + bl cordic_vstep + bcc 1b // step until table finished +4: + cmp r1,#0 // continue as in cordic_vstep but without using table; x is not affected as y is small + bgt 2f // check sign of y + adds r1,r6 // positive rotation: y=y+(x>>i) + subs r2,r4 // accumulate angle + b 3f +2: + subs r1,r6 // negative rotation: y=y-(x>>i) + adds r2,r4 // accumulate angle +3: + asrs r6,#1 + asrs r4,#1 // next "table entry" + bne 4b + b cordic_exit + +.thumb_func +qfp_fsin: // calculate sin and cos using CORDIC rotation method + push {r4,r5,r14} + movs r1,#24 + bl qfp_float2fix // range reduction by repeated subtraction/addition in fixed point + ldr r4,pi_q29 + lsrs r4,#4 // 2pi Q24 +1: + subs r0,r4 + bge 1b +1: + adds r0,r4 + bmi 1b // now in range 0..2pi + lsls r2,r0,#2 // z Q26 + lsls r5,r4,#1 // pi Q26 (r4=pi/2 Q26) + ldr r0,=#0x136e9db4 // initialise CORDIC x,y with scaling + movs r1,#0 +1: + cmp r2,r4 // >pi/2? + blt 2f + subs r2,r5 // reduce range to -pi/2..pi/2 + rsbs r0,#0 // rotate vector by pi + b 1b +2: + lsls r2,#3 // Q29 + adr r3,tab_cc // circular coefficients + movs r4,#1 // m=1 + bl cordic_rot + adds r1,#9 // fiddle factor to make sin(0)==0 + movs r2,#0 // exponents to zero + movs r3,#0 + movs r5,#0 // no sticky bits + bl packx // pack cosine + bl xchxy + b packretns // pack sine + +.thumb_func +qfp_fcos: + push {r14} + bl qfp_fsin + mov r0,r1 // extract cosine result + pop {r15} + + +.thumb_func +qfp_ftan: + push {r4,r5,r14} + bl qfp_fsin // sine in r0/r2, cosine in r1/r3 +.if include_faster + b fdiv_fast_n // sin/cos +.else + b fdiv_n + +.endif + +.thumb_func +qfp_fexp: // calculate cosh and sinh using rotation method; add to obtain exp + push {r4,r5,r14} + movs r1,#24 + bl qfp_float2fix // Q24: covers entire valid input range + asrs r1,r0,#16 // Q8 + ldr r2,=#5909 // log_2(e) Q12 + muls r1,r2 // estimate exponent of result Q20 + asrs r1,#19 // Q1 + adds r1,#1 // rounding + asrs r1,#1 // rounded estimate of exponent of result + push {r1} // save for later + lsls r2,r0,#5 // Q29 + ldr r0,=#0x162e42ff // ln(2) Q29 + muls r1,r0 // accurate contribution of estimated exponent + subs r2,r1 // residual to be exponentiated, approximately -.5..+.5 Q29 + ldr r0,=#0x2c9e15ca // initialise CORDIC x,y with scaling + movs r1,#0 + adr r3,tab_ch // hyperbolic coefficients + mvns r4,r1 // m=-1 + bl cordic_rot // calculate cosh and sinh + add r0,r1 // exp=cosh+sinh + pop {r2} // recover exponent + b packretns // pack result + +.thumb_func +qfp_fsqrt: // calculate sqrt and ln using vector method + push {r4,r5,r14} + bl unpackx + movs r1,r0 // -ve argument? + bmi 3f // return -Inf, -Inf + ldr r1,=#0x0593C2B9 // scale factor for CORDIC + bl mul0 // Q29 + asrs r1,r2,#1 // halve exponent + bcc 1f + adds r1,#1 // was odd: add 1 and shift mantissa + asrs r0,#1 +1: + push {r1} // save exponent/2 for later + mov r1,r0 + ldr r3,=#0x0593C2B9 // re-use constant + lsls r3,#2 + adds r0,r3 // "a+1" + subs r1,r3 // "a-1" + movs r2,#0 + adr r3,tab_ch // hyperbolic coefficients + mvns r4,r2 // m=-1 + bl cordic_vec + mov r1,r2 // keep ln result + pop {r2} // retrieve exponent/2 +2: + movs r3,r2 + b packretns // pack sqrt result + +3: + movs r2,#255 + b 2b + +.thumb_func +qfp_fln: + push {r4,r5,r14} + bl qfp_fsqrt // get unpacked ln in r1/r3; exponent has been halved + cmp r3,#70 // ln(Inf)? + bgt 3b // return Inf + rsbs r3,#0 + cmp r3,#70 + bgt 1f // ln(0)? return -Inf + ldr r0,=#0x0162e430 // ln(4) Q24 + muls r0,r3 // contribution from negated, halved exponent + adds r1,#8 // round result of ln + asrs r1,#4 // Q24 + subs r0,r1,r0 // add in contribution from (negated) exponent + movs r2,#5 // pack expects Q29 + b packretns +1: + mvns r0,r0 // make result -Inf + b 3b + +.thumb_func +qfp_fatan2: + push {r4,r5,r14} + bl unpackxyalign // convert to fixed point (ensure common exponent, which is discarded) + asrs r0,#1 + asrs r1,#1 + movs r2,#0 // initial angle + cmp r0,#0 // x negative + bge 5f + rsbs r0,#0 // rotate to 1st/4th quadrants + rsbs r1,#0 + ldr r2,pi_q29 // pi Q29 +5: + adr r3,tab_cc // circular coefficients + movs r4,#1 // m=1 + bl cordic_vec // also produces magnitude (with scaling factor 1.646760119), which is discarded + mov r0,r2 // result here is -pi/2..3pi/2 Q29 + ldr r2,pi_q29 // pi Q29 + adds r4,r0,r2 // attempt to fix -3pi/2..-pi case + bcs 6f // -pi/2..0? leave result as is + subs r4,r0,r2 // <pi? leave as is + bmi 6f + subs r0,r4,r2 // >pi: take off 2pi +6: + subs r0,#1 // fiddle factor so atan2(0,1)==0 + movs r2,#0 // exponent for pack + b packretns + +.align 2 +.ltorg + +// first entry in following table is pi Q29 +pi_q29: +// circular CORDIC coefficients: atan(2^-i), b0=flag for preventing shift, b1=flag for end of table +tab_cc: +.word 0x1921fb54*4+1 // no shift before first iteration +.word 0x0ed63383*4+0 +.word 0x07d6dd7e*4+0 +.word 0x03fab753*4+0 +.word 0x01ff55bb*4+0 +.word 0x00ffeaae*4+0 +.word 0x007ffd55*4+0 +.word 0x003fffab*4+0 +.word 0x001ffff5*4+0 +.word 0x000fffff*4+0 +.word 0x0007ffff*4+0 +.word 0x00040000*4+0 +.word 0x00020000*4+0+2 // +2 marks end + +// hyperbolic CORDIC coefficients: atanh(2^-i), flags as above +tab_ch: +.word 0x1193ea7b*4+0 +.word 0x1193ea7b*4+1 // repeat i=1 +.word 0x082c577d*4+0 +.word 0x04056247*4+0 +.word 0x0200ab11*4+0 +.word 0x0200ab11*4+1 // repeat i=4 +.word 0x01001559*4+0 +.word 0x008002ab*4+0 +.word 0x00400055*4+0 +.word 0x0020000b*4+0 +.word 0x00100001*4+0 +.word 0x00080001*4+0 +.word 0x00040000*4+0 +.word 0x00020000*4+0 +.word 0x00020000*4+1+2 // repeat i=12 + +.endif + +qfp_lib_end: |