uss_fpfncs.s

Vxworks OS source code

        moveb   a7@(7),d0
        jeq     FLN060          | J/ center = 0 -> no additive log val
|
        pea     FLNLOG-4
        movel   sp@+,a6
        movel   a6@(0,d0:W),sp@-
        jsr     FPADD           | Add in center log value
|
FLN060:
        movew   a7@(4),d0       | /* Get two's exponent value */
        extl    d0
| ##    DC.W    $48C0           ; (Assembled  EXT.L D0 instruction)
        jsr     FLOAT
|
        movel   FLN2,sp@-       | Log of 2 on stack
        jsr     FPMUL
        jsr     FPADD
|
        tstb    a7@(6)
        jeq     FLN070          | J/ natural log
|
        movel   FILN10,sp@-     | Scaling value
        jsr     FPMUL
|
FLN070:
        movel   sp@+,sp@
        jmp     a4@
/*
| ###   SUBTTL  FPXTOI: Floating Point Number to Integer Power Function
|       page
|
|  X to I power Function.
|
|  The single precision floating point value on the stack is
|  raised to the power specified in D0.W then returned on stack.
|
|  A shift and multiply technique is used (possibly with a trailing
|  recipication.
|
|
| ###   PUBLIC  FPXTOI
|
*/

FPXTOI:
        |dsw    0
        bsr     FPFADJ
|
        jvs     FFNANR          | J/ NaN arg -> NaN result
        jcc     FPXT10          | J/ arg is not INF
|
        tstw    d0
        jeq     FFNANR          | J/ arg is +/- INF, I is 0 -> NaN
        jmi     FFUNFR          | J/ x is +/- INF, I < 0 -> underflow
|
        rorb    #1,d0
        andb    sp@,d0
        jmi     FFMINR          | J/ arg is -INF, I is +odd -> -INF
        jra     FFPINR          | else -> +INF
|
|
FPXT10:
        jne     FPXT15          | J/ parm is a number <> 0.0
|
        tstw    d0
        jmi     FFPINR          | J/ 0.0 to -int -> +INF
        jeq     FFNANR          | J/ 0.0 to 0 -> NaN
        jra     FFZERR          | J/ 0.0 to +int -> 0.0
|
FPXT15:
        tstw    d0
        jeq     FFONER          | J/ num to 0 -> 1.0
|
        movew   d0,a7@(6)       | Save int as its own sign flag
        jpl     FPXT20
        negw    d0
FPXT20:
        |dsw    0
|
        movel   sp@,sp@-        | Result init to parm value
|
        moveq   #16,d1          | Find MS bit of power
FPXT21:
        lslw    #1,d0
        jcs     FPXT22          | J/ MS bit moved into carry
        dbra    d1,FPXT21       | Dec d1 and jump (will always jump)
|
FPXT22:
        movew   d0,a7@(8)       | Power pattern on stack
        moveb   d1,a7@(11)      | Bit slots left count on stack
|
|
FPXT30:
        subqb   #1,a7@(11)      | Decrement bit slots left
        jeq     FPXT35          | J/ evaluation complete
|
        movel   sp@,sp@-        | Square result value
        jsr     FPMUL           | Square it
|
        lslw    a7@(8)          | Shift power pattern
        jcc     FPXT30          | J/ this product bit not set
|
        movel   a7@(4),sp@-     | Copy parm value
        jsr     FPMUL
        jra     FPXT30
|
FPXT35:
        tstb    a7@(10)         | Check for recipocation
        jpl     FPXT36          | J/ no recipocation
|
        movel   #0x3F800000,sp@- |Place 1.0 on stack
        jsr     FPRDIV
|
FPXT36:
        movel   sp@+,a7@(4)     | Shift result value
        addql   #4,sp           | Delete excess stack area
        jmp     a4@             | Return.
/*
|
| ###   SUBTTL  FPSQRT: Square Root Function
|       page
|
|  Square Root Function.
|
|  Take square root of the single precision floating point value
|  on the top of the stack.
|
|  Use the Newton iteration technique to compute the square root.
|
|      X(n+1) = (X(n) + Z/X(n)) / 2
|
|  The two*s exponent is scaled to restrict the solution domain to 1.0
|  through 4.0.  A linear approximation to the square root is used to
|  produce a first guess with greater than 4 bits of accuracy.  Three
|  successive iterations are performed in registers to obtain accuracy
|  of about 30 bits.  The final iteration is performed in the floating
|  point domain.
|
| ###   PUBLIC  FPSQRT
|
*/

FPSQRT:
        |dsw    0
        bsr     FPFADJ
|
        jvs     FFNANR          | J/ NaN arg -> NaN result
        jmi     FFNANR          | J/ neg arg -> NaN result
        jcs     FFPINR          | J/ +INF arg -> +INF result
        jeq     FFZERR          | J/ 0.0 arg -> 0.0 result
|
        movew   sp@,d1          | Get S/E/M word
        subiw   #128*FBIAS,d1   | Extract argument's two's exp
        clrb    d1              | Make it a factor of two
        subw    d1,sp@          | /* Scale arg. range to 4.0 > arg' >= 1.0 */
        asrw    #8,d1           | Square root of scaled two power
        moveb   d1,a7@(4)       | /* Save two's exp of result on stack */
|
        movel   sp@,d1          | Create fixed point integer for approx
        lsll    #8,d1           | /* Produce arg' * 2^30 in d1 */
        bset    #31,d1          | Set implicit bit
        jeq     FPSQ10          | /* J/ arg' >= 2.0 */
|
        lsrl    #1,d1           | Adjust d1
|
FPSQ10:
        movew   #42720-65536,d2 | d2 = 0.325926 * 2^17
        swap    d1              | /* d1.W = arg' * 2^14 */
        mulu    d1,d2           | /* d2 = arg' * 0.325926 * 2^31 */
        swap    d2
        addiw   #23616,d2       | + 0.7207 * 2^15 - to 4+ bits
        subxw   d3,d3
        orw     d3,d2           | Top out approximation at 1.99997
|
        swap    d1
        lsrl    #1,d1           | /* Arg' * 2^29 in d1 (prevent overflow) */
|
        movel   d1,d3           | Copy into d3
        divu    d2,d3           | /* Arg'/X0 * 2^14 in d3 */
        lsrw    #1,d2
        addw    d3,d2           | X1 in d2 - to 8 bits
|
        movel   d1,d3           | Second in-register iteration
        divu    d2,d3
        lsrw    #1,d2
        addw    d3,d2           | X2 in d2 - to 16 bits
|
        movel   d1,d3
        divu    d2,d3
        movew   d3,d4
        clrw    d3
        swap    d4
        divu    d2,d3
        movew   d3,d4           | 32 bit division result
        swap    d2
        clrw    d2
        lsrl    #1,d2
        addl    d4,d2           | X3 in d2 - to 24+ bits
|
        moveq   #0x7F,d1
        addl    d1,d2           | Round value in d2 to 24 bits
        roll    #1,d2           | Strip implicit bit
|       .set    XBIAS,127               | *** for assembler bug ***
        moveb   #XBIAS,d2       | Place bias value
        addb    a7@(4),d2       | Scale result
        rorl    #8,d2           | Position FP value
        lsrl    #1,d2           | Force sign bit to 0
        addql   #4,sp           | Delete four bytes from the stack
        movel   d2,sp@
        jmp     a4@
/*
|
| ###   SUBTTL  FPATN: Arctangent Function
|       page
|
|  ARCTANGENT Function.
|
|  The arctangent of the single precision floating point value on
|  the stack computed by using a split domain with a polynomial
|  approximation.  A principal range radian value is returned.
|
|  The domain is split at 0.25 (four) intervals.  A polynomial is
|  used to approximate the arctangent for magnitudes less than 1/8.
|
|  Using the trigonometric identity:
|         ARCTAN(y) + ARCTAN(z) = ARCTAN((y+z)/(1+yz))
|         If z = (x-y)/(1+xy) then ARCTAN((y+z)/(1+yz)) = ARCTAN(x).
|
|         ARCTAN(-v) = -ARCTAN(v)        * make argument positive
|         ARCTAN(1/v) = PI/2 - ARCTAN(v) * reduce argument to <= 1.0
|
|
|  ARCTANGENT approximation polynomical coefficients
|
|         C3  =   -1.4285 71429E-01     = -1/7
*/

FATNCN:
        .long   0xBE124925
|         C2  =    2.0000 00000E-01     =  1/5
        .long   0x3E4CCCCD
|         C1  =   -3.3333 33333E-01     = -1/3
        .long   0xBEAAAAAB
|         C0  =    1.0000 00000E+00     =  1/1
        .long   0x3F800000
|
|       .set    NFATNC,4
|
|
|
|  Table of ARCTANGENT values at 0.125 intervals
|
|         ATAN(1/4)  =  2.4497 86631E-01
FATNTB:
        .long   0x3E7ADBB0
|         ATAN(2/4)  =  4.6364 76090E-01
        .long   0x3EED6338
|         ATAN(3/4)  =  6.4350 11088E-01
        .long   0x3F24BC7D
|         ATAN(1/1)  =  7.8539 81634E-01   ( = PI/4)
        .long   0x3F490FDB
|
|
|  FP PI/2 (duplicate of FPIO2 because of assembler bug)
|
FXPIO2:
        .long   0x3FC90FDB
|
| ###   PUBLIC  FPATN
|
FPATN:
        |dsw    0
        bsr     FPFADJ
|
        jvs     FFNANR          | J/ NaN arg -> NaN result
        jcc     FPAT10          | J/ not INF
|
        movel   FXPIO2,d1       | Get PI/2
        roxll   #1,d1
        roxlw   sp@             | INF sign bit into X
        roxrl   #1,d1           | PI/2 given sign of INF
        addql   #4,sp           | Delete four bytes from the stack
        movel   d1,sp@
        jmp     a4@
|
FPAT10:
        jeq     FFZERR          | J/ 0.0 arg -> 0.0 result
|
        bclr    #7,sp@          | Insure argument positive
        sne     d0              | Create flag byte (0xFF iff negative)
        andib   #0x80,d0                | Keep sign bit only
        moveb   d0,a7@(6)       | Save flag byte
|
        movew   sp@,d1
        cmpiw   #128*FBIAS+0x10,d1      | (top word of 1.125)
        jlt     FPAT20                  | J/ arg < 1 + 1/8
|
        addqb   #1,a7@(6)
|
        movel   #0x3F800000,sp@- |Place 1.0 onstack
        jsr     FPRDIV          | Invert the number
|
FPAT20:
        movew   sp@,d1
        cmpiw   #128*FBIAS-256,d1
        jlt     FPAT30          | J/ arg < 1/4
|
        moveb   d1,d2           | Number of eights in d2
        orib    #0x80,d2                | Implicit bit
        lsrw    #7,d1
        moveq   #FBIAS+4-256,d3
        subb    d1,d3
        lsrb    d3,d2
        addqb   #1,d2
        lsrb    #1,d2           | Rounded quarters in d2.B
|
        clrl    d0
        moveb   d2,d0           | 32 bit integer in d0
|
        lslb    #2,d2
        addb    d2,a7@(6)       | Save 4 * quarters on stack
|
        jsr     FLOAT
        subiw   #128*2,sp@      | Produce y, floating point quarters
|
        movel   a7@(4),sp@-
        movel   a7@(4),sp@-     | On stack:  y, arg', y, arg', <temps>
|
        jsr     FPMUL           | /* arg'*y */
        movel   #0x3F800000,sp@-
        jsr     FPADD           | /* 1.0 + arg'*y */
|
        movel   a7@(8),d0       | Exchange stack items
        movel   sp@,a7@(8)
        movel   d0,sp@          | On stack: arg', y, (1+arg'*y), <tmps>
        bset    #7,a7@(4)       | Negate y
        jsr     FPADD           | /* (arg'-y) */
        jsr     FPRDIV          | (arg'-y) / (1 + arg'*y)
|
FPAT30:
        |dsw    0
        movel   sp@,sp@-        | Duplicate z
|
        pea     FATNCN
        movel   sp@+,a6 | Polynomial approximation to small ATN
        moveq   #NFATNC,d0
        bsr     FX2SER
        jsr     FPMUL           | Complete approximation
|
        moveb   a7@(6),d0
        andiw   #0x001C,d0      | Trim to table index
        jeq     FPAT40          | J/ y = 0.0, ARCTAN(y) = 0.0
|
        pea     FATNTB-4
        movel   sp@+,a6
        movel   a6@(0,d0:W),sp@-
        jsr     FPADD           | Add in ARCTAN(y)
|
FPAT40:
        btst    #0,a7@(6)       | Check for inversion
        jeq     FPAT50          | J/ no inversion
|
        bset    #7,sp@          | Negate ARCTAN
        movel   FXPIO2,sp@-
        jsr     FPADD           | Inversion via subtraction
|
FPAT50:
        tstb    a7@(6)          | Check sign of result
        jpl     FPAT60          | J/ positive
|
        bset    #7,sp@          | Negate result
|
FPAT60:
        movel   sp@+,sp@        | Downshift result
        jmp     a4@
/*
|
| ###   SUBTTL  FPCOS, FPSIN, FPTAN: Trigonometric Functions
|       page
|
|  TRIG ROUTINES.
|
|  The support routine FTRGSV converts the radian mode argument to
|  an quadrant value between -0.5 and 0.5 (quadrants).  The sign of the
|  is saved (the argument is forced non-negative).  Computations are
|  performed for values quadrant values of -0.5 to 0.5* other values
|  are transformed as per the following transformation formulae
|  and table:
|
|     Let:     z = shifted quadrant value
|     Recall:  sin(-x) = -sin(x)    tan(-x) = -tan(x)
|              cos(-x) =  cos(x)
|
|     l========l=================l=================l=================l