// Copyright 2015 Mark Owen (qfp@quinapalus.com) // 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>20 Q7 ldrb r4,[r4,r3] // m=rcpapp[(y>>20)&7]; // Q8, .5>=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: 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: