uss_dpfncs.s

VXWORKS源代码

        movel   a6@(4,d0:W),sp@-
        movel   a6@(0,d0:W),sp@-
        jsr     DPADD           | Add in center log value
|
DLN060:
        movew   a7@(8),d1       | /* Get two's exponent value */
        clrl    d0
        extl    d1
        jpl     DLN061
        moveq   #-1,d0
DLN061:
        |dsw    0
|
        jsr     DFLOAT
|
        movel   DLN2+4,sp@-     | Log of 2 on stack
        movel   DLN2+0,sp@-
        jsr     DPMUL
        jsr     DPADD
|
        tstb    a7@(10)
        jeq     DLN070          | J/ natural log
|
        movel   DILN10+4,sp@-   | Scaling value
        movel   DILN10+0,sp@-
        jsr     DPMUL
|
DLN070:
        movel   a7@(4),a7@(8)
        movel   sp@+,sp@
        jmp     a4@

/*
| ###   SUBTTL  DPXTOI: Floating Point Number to Integer Power Function
|       page
|
|  X to I power Function.
|
|  The double 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  DPXTOI
|
*/

DPXTOI:
        |dsw    0
        bsr     DPFADJ
|
        jvs     DFNANR          | J/ NaN arg -> NaN result
        jcc     DPXT10          | J/ arg is not INF
|
        tstw    d0
        jeq     DFNANR          | J/ arg is +/- INF, I is 0 -> NaN
        jmi     DFUNFR          | J/ x is +/- INF, I < 0 -> underflow
|
        rorw    #1,d0
        andw    sp@,d0
        jmi     DFMINR          | J/ arg is -INF, I is +odd -> -INF
        jra     DFPINR          | else -> +INF
|
|
DPXT10:
        jne     DPXT15          | J/ parm is a number <> 0.0
|
        tstw    d0
        jmi     DFPINR          | J/ 0.0 to -int -> +INF
        jeq     DFNANR          | J/ 0.0 to 0 -> NaN
        jra     DFZERR          | J/ 0.0 to +int -> 0.0
|
DPXT15:
        tstw    d0
        jeq     DFONER          | J/ num to 0 -> 1.0
|
        movew   d0,a7@(10)      | Save int as its own sign flag
        jpl     DPXT20
        negw    d0
DPXT20:
        |dsw    0
|
        movel   a7@(4),sp@-     | Result init to parm value
        movel   a7@(4),sp@-
|
        moveq   #16,d1          | Find MS bit of power
DPXT21:
        lslw    #1,d0
        jcs     DPXT22          | J/ MS bit moved into carry
        dbra    d1,DPXT21       | Dec d1 and jump (will always jump)
|
DPXT22:
        movew   d0,a7@(16)      | Power pattern on stack
        moveb   d1,a7@(19)      | Bit slots left count on stack
|
|
DPXT30:
        subqb   #1,a7@(19)      | Decrement bit slots left
        jeq     DPXT35          | J/ evaluation complete
|
        movel   a7@(4),sp@-     | Square result value
        movel   a7@(4),sp@-
        jsr     DPMUL           | Square it
|
        lslw    a7@(16)         | Shift power pattern
        jcc     DPXT30          | J/ this product bit not set
|
        movel   a7@(12),sp@-    | Copy parm value
        movel   a7@(12),sp@-
        jsr     DPMUL
        jra     DPXT30
|
DPXT35:
        tstb    a7@(18)         | Check for recipocation
        jpl     DPXT36          | J/ no recipocation
|
        clrl    sp@-            | Place 1.0 on stack
        movel   #0x3FF00000,sp@-
        jsr     DPRDIV
|
DPXT36:
        movel   sp@+,a7@(8)     | Shift result value
        movel   sp@+,a7@(8)
        addql   #4,sp           | Delete excess stack area
        jmp     a4@             | Return.
/*
|
| ###   SUBTTL  DPSQRT: Square Root Function
|       page
|
|  Square Root Function.
|
|  Take square root of the double 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  DPSQRT
|
*/

DPSQRT:
        |dsw    0
        bsr     DPFADJ
|
        jvs     DFNANR          | J/ NaN arg -> NaN result
        jmi     DFNANR          | J/ neg arg -> NaN result
        jcs     DFPINR          | J/ +INF arg -> +INF result
        jeq     DFZERR          | J/ 0.0 arg -> 0.0 result
|
        movew   sp@,d1          | Get S/E/M word
        subiw   #16*DBIAS,d1    | Extract argument's two's exp
        andib   #0xE0,d1                | Make it a factor of two
        subw    d1,sp@          | /* Scale arg. range to 4.0 > arg' >= 1.0 */
        asrw    #1,d1           | Square root of scaled two power
        movew   d1,a7@(8)       | /* Save two's exp of result on stack */
|
        movel   sp@,d1          | Create fixed point integer for approx
        movew   a7@(4),d2
        lsll    #8,d1           | /* Produce arg' * 2^30 in d1 */
        lsll    #3,d1
        lsrl    #5,d2
        orw     d2,d1
        bset    #31,d1          | Set implicit bit
        jeq     DPSQ10          | /* J/ arg' >= 2.0 */
|
        lsrl    #1,d1           | Adjust d1
|
DPSQ10:
        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 29 bits
        subxl   d4,d4
        orl     d4,d2           | Top out at 1.9999999995
|
        movel   a7@(4),sp@-     | /* Down shift arg' */
        movel   a7@(4),sp@-
|
        lsll    #1,d2           | Create DP of X3 (good to 29 bits) ...
        clrl    d3              | ... in d2:d3
        movew   d2,d3
        andiw   #0xFFF,d3
        eorw    d3,d2
        oriw    #DBIAS,d2       | Scale to floating point
        moveq   #12,d0          | Shift count in d0
        rorl    d0,d2           | Position bits
        rorl    d0,d3
|
        movel   d2,a7@(8)       | /* On stack: X4, arg', X4, <flags> */
        movel   d3,a7@(12)
        movel   d3,sp@-
        movel   d2,sp@-
        jsr     DPDIV           | Last iter - to X4 - in DP domain
        jsr     DPADD
        subiw   #0x0010,sp@     | "Divide" by 2
|
        movew   a7@(8),d7
        addw    d7,sp@          | Scale result
        movel   a7@(4),a7@(8)   | Down shift result
        movel   sp@+,sp@
        jmp     a4@

/*
| ###   SUBTTL  DPATN: Arctangent Function
|       page
|
|  ARCTANGENT Function.
|
|  The arctangent of the double precision floating point value at SI is
|  computed by using a split domain with a polynomial approximation.
|  A principal range radian value is returned.
|
|  The domain is split at 0.125 (eight) intervals.  A polynomial is
|  used to approximate the arctangent for magnitudes less than 1/16.
|
|  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
|
|         C4  =    1.1022 81616 12614 90000E-01
*/

DATNCN:
        .long   0x3FBC37E9,0xAD397134
|         C3  =   -1.4285 41305 08745 00000E-01
        .long   0xBFC2490B,0x4D511901
|         C2  =    1.9999 99958 01446 40000E-01
        .long   0x3FC99999,0x90956BB6
|         C1  =   -3.3333 33333 31284 50000E-01
        .long   0xBFD55555,0x5554C529
|         C0  =    1.0000 00000 00000 00000E+00
        .long   0x3FF00000,0x00000000
|
|       .set    NDATNC,5
|
|
|
|  Table of ARCTANGENT values at 0.125 intervals
|
|         ATAN(1/8)  =  1.2435 49945 46761 43503E-01
DATNTB:
        .long   0x3FBFD5BA,0x9AAC2F6E
|         ATAN(2/8)  =  2.4497 86631 26864 15417E-01
        .long   0x3FCF5B75,0xF92C80DE
|         ATAN(3/8)  =  3.5877 06702 70572 22040E-01
        .long   0x3FD6F619,0x41E4DEF1
|         ATAN(4/8)  =  4.6364 76090 00806 11621E-01
        .long   0x3FDDAC67,0x0561BB4F
|         ATAN(5/8)  =  5.5859 93153 43562 43597E-01
        .long   0x3FE1E00B,0xABDEFEB4
|         ATAN(6/8)  =  6.4350 11087 93284 38680E-01
        .long   0x3FE4978F,0xA3269EE1
|         ATAN(7/8)  =  7.1882 99996 21624 50542E-01
        .long   0x3FE700A7,0xC5784634
|         ATAN(8/8)  =  7.8539 81633 97448 30962E-01   ( = PI/4)
        .long   0x3FE921FB,0x54442D18
|
|
|  DP PI/2 (duplicate of DPIO2 because of assembler bug)
|
DXPIO2:
        .long   0x3FF921FB,0x54442D18
|
| ###   PUBLIC  DPATN
|
DPATN:
        |dsw    0
        bsr     DPFADJ
|
        jvs     DFNANR          | J/ NaN arg -> NaN result
        jcc     DPAT10          | J/ not INF
|
        movel   DXPIO2+0,d1     | Get top long word of 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   DXPIO2+4,a7@(4)
        movel   d1,sp@
        jmp     a4@
|
DPAT10:
        jeq     DFZERR          | 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@(10)      | Save flag byte
|
        movew   sp@,d1
        cmpiw   #16*DBIAS,d1
        jle     DPAT20          | J/ arg < 1 + 1/16
|
        addqb   #1,a7@(10)
|
        clrl    sp@-            | Place 1.0 onstack
        movel   #0x3FF00000,sp@-
        jsr     DPRDIV          | Invert the number
|
DPAT20:
        movew   sp@,d1
        cmpiw   #16*DBIAS-64,d1
        jlt     DPAT30          | J/ arg < 1/16
|
        moveb   d1,d2           | Number of sixteenths in d2
        andib   #0x0F,d2
        orib    #0x10,d2                | Implicit bit
        lsrw    #4,d1
        moveq   #-1,d3
        subb    d1,d3
        lsrb    d3,d2
        addqb   #1,d2
        lsrb    #1,d2           | Rounded eighths in d2.B
|
        clrl    d0
        clrl    d1
        moveb   d2,d1           | 64 bit integer in d0:d1
|
        lslb    #3,d2
        addb    d2,a7@(10)      | Save 8 * eigths on stack
|
        jsr     DFLOAT
        subiw   #16*3,sp@       | Produce y, floating point eighths
|
        moveq   #4-1,d0
DPAT25:
        movel   a7@(12),sp@-
        dbra    d0,DPAT25       | On stack:  y, arg', y, arg', <temps>
|
        jsr     DPMUL           | /* arg'*y */
        clrl    sp@-            | Place 1.0 on stack
        movel   #0x3FF00000,sp@-
        jsr     DPADD           | /* 1.0 + arg'*y */
|
        movel   a7@(16),d0      | Exchange stack item
        movel   a7@(20),d1
        movel   sp@,a7@(16)
        movel   a7@(4),a7@(20)
        movel   d0,sp@
        movel   d1,a7@(4)       | On stack: arg', y, (1+arg'*y), <tmps>
        bset    #7,a7@(8)       | Negate y
        jsr     DPADD           | /* (arg'-y) */
        jsr     DPRDIV          | /* (arg'-y) / (1 + arg'*y) */
|
DPAT30:
        |dsw    0
        movel   a7@(4),sp@-     | Duplicate z
        movel   a7@(4),sp@-
|
        pea     DATNCN
        movel   sp@+,a6 | Polynomial approximation to small ATN
        moveq   #NDATNC,d0
        bsr     DX2SER
        jsr     DPMUL           | Complete approximation
|
        moveb   a7@(10),d0
        andiw   #0x0078,d0      | Trim to table index
        jeq     DPAT40          | J/ y = 0.0, ARCTAN(y) = 0.0
|
        pea     DATNTB-8
        movel   sp@+,a6
        movel   a6@(4,d0:W),sp@-
        movel   a6@(0,d0:W),sp@-
        jsr     DPADD           | Add in ARCTAN(y)
|
DPAT40:
        btst    #0,a7@(10)      | Check for inversion
        jeq     DPAT50          | J/ no inversion
|
        bset    #7,sp@          | Negate ARCTAN
        movel   DXPIO2+4,sp@-
        movel   DXPIO2+0,sp@-
        jsr     DPADD           | Inversion via subtraction
|
DPAT50:
        tstb    a7@(10)         | Check sign of result
        jpl     DPAT60          | J/ positive
|
        bset    #7,sp@          | Negate result
|
DPAT60:
        movel   a7@(4),a7@(8)   | Downshift result
        movel   sp@+,sp@
        jmp     a4@

/*
|
| ###   SUBTTL  DPCOS, DPSIN, DPTAN: Trigonometric Functions
|       page
|
|  TRIG ROUTINES.
|
|  The support routine DTRGSV converts the radian mode argument to
|  an quadrant value between -0.5 and 0.5 (quadrants).  The sign of the