uss_fpfncs.s

Vxworks OS source code

|     l  QUAD  l       SIN       l       COS       l       TAN       l
|     l========l=================l=================l=================l
|     l    0   l      sin(z)     l      cos(z)     l      tan(z)     l
|     l    1   l      cos(z)     l     -sin(z)     l   -1/tan(z)     l
|     l    2   l     -sin(z)     l     -cos(z)     l      tan(z)     l
|     l    3   l     -cos(z)     l      sin(z)     l   -1/tan(z)     l
|     l========l=================l=================l=================l
|
|  The sign of the argument is, by the equations above, important only
|  when computing SIN and TAN.
|
|  Chebyshev Polynomials are used to approximate the trigonometric
|  value in the range -0.5 to 0.5 quandrants.  (Note: constants are
|  adjusted to reflect calculations done in quadrant mode instead
|  of radian mode).
|
|
|
|         C08  =   9.1926 02748E-04
*/

FCOSCN:
        .long   0x3A70FA83
|         C06  =  -2.0863 48076E-02
        .long   0xBCAAE9E4
|         C04  =   2.5366 95079E-01
        .long   0x3E81E0F8
|         C02  =  -1.2337 00550E+00
        .long   0xBF9DE9E6
|         C00  =   1.0000 00000E+00
        .long   0x3F800000
|
|       .set    NFCOSC,5
|
|
|
|         C09  =   1.6044 11848E-04
FSINCN:
        .long   0x39283C1A
|         C07 =   -4.6817 54135E-03
        .long   0xBB996966
|         C05  =   7.9692 62625E-02
        .long   0x3DA335E3
|         C03  =  -6.4596 40975E-01
        .long   0xBF255DE7
|         C01  =   1.5707 96327E+00
FPIO2:
        .long   0x3FC90FDB
|
|       .set    NFSINC,5
|
|
|
|         Q3  =    1.7732 32244E+01
FTNQCN:
        .long   0x418DDBCC
|         Q2  =   -8.0485 82645E+02
        .long   0xC44936EE
|         Q1  =    6.4501 55566E+03
        .long   0x45C9913F
|         Q0  =   -5.6630 29631E+03
        .long   0xC5B0F83D
|
|       .set    NFTNQC,4
|
|
|         P3  =    1.0000 00000E+00
FTNPCN:
        .long   0x3F800000
|         P2  =   -1.5195 78276E+02
        .long   0xC317F534
|         P1  =    2.8156 53022E+03
        .long   0x452FFA73
|         P0  =   -8.8954 66142E+03
        .long   0xC60AFDDD
|
|       .set    NFTNPC,4
|
|
|         INV2PI  =  1 / 2*PI  =   1.5915 49431E-01
FIN2PI:
        .long   0x3E22F983
|
|
| ###   PUBLIC  FPCOS
|
FPCOS:
        bsr     FTRGSV          | Prepare argument for operation
|
        moveb   d0,d1           | Copy into d1
        andib   #3,d0           | Strip sign bit
        addqb   #1,d0
        rorb    #2,d0           | Move sign bit to B7
        moveb   d0,a7@(4)       | Save it
        rorb    #1,d1
        jcs     FSINOP          | Compute sine
|
|
FCOSOP:
        pea     FCOSCN
        movel   sp@+,a6
        moveq   #NFCOSC,d0
        bsr     FX2SER
|
FTRGFN:
        moveb   a7@(4),d0
        andib   #0x80,d0                | Isolate sign bit
        eorb    d0,sp@
|
        movel   sp@+,sp@
        jmp     a4@
|
|
|
| ###   PUBLIC  FPSIN
|
FPSIN:
        bsr     FTRGSV          | Prepare argument
|
        addib   #0x7E,a7@(4)    | Compute sign
        rorb    #1,d0
        jcs     FCOSOP
|
|
FSINOP:
        movel   sp@,sp@-        | Duplicate argument
|
        pea     FSINCN
        movel   sp@+,a6
        moveq   #NFSINC,d0
        bsr     FX2SER
        jsr     FPMUL
|
        jra     FTRGFN
|
|
|
| ###   PUBLIC  FPTAN
|
FPTAN:
        bsr     FTRGSV          | Prepare argument
|
        rorb    #1,d0
        andib   #0x80,d0
        eorb    d0,a7@(4)
|
        movel   sp@,sp@-        | Double duplication of argument
        movel   sp@,sp@-
|
        pea     FTNPCN
        movel   sp@+,a6
        moveq   #NFTNPC,d0
        bsr     FX2SER
        jsr     FPMUL
|
        movel   a7@(4),d0
        movel   sp@,a7@(4)
        movel   d0,sp@
|
        pea     FTNQCN
        movel   sp@+,a6
        moveq   #NFTNQC,d0
        bsr     FX2SER
|
        btst    #0,a7@(8)
        jne     FPTN20          | J/ reverse division
|
        jsr     FPDIV
        jra     FTRGFN
|
FPTN20:
        jsr     FPRDIV
        jra     FTRGFN
/*
|
|       page
|
|  Trigonometric Service Routine
|
|  The single precision floating point value to be processed is on the
|  stack.  Excess 2 PI's are removed from the argument.  The range of
|  the argument is then scaled to within PI/4 of 0.  The shift size
|  (in number of quadrants) is returned in D0.  The original sign bit
|  is returned in the D0.B sign bit.
|
|  If the argument is NaN, +INF, -INF, or too large (>= 2^16), this
|  routine causes a return to the caller with a NaN result.
|
*/

FTRGSV:
        moveal  sp@+,a6 | FTRGSV return addr
|
        bsr     FPFADJ
        jcs     FFNANR          | Return NAN if +/- INF
        jvs     FFNANR          | Return NaN if NaN
|
        clrw    a7@(4)          | Create arg type return byte
|
        roxlw   sp@             | Sign bit into X
        roxrw   a7@(4)          | Sign bit into temp byte
        roxrw   sp@             | Restore FP value (with sign = +)
|
        cmpiw   #128*FBIAS+2048,sp@     | (2048 = 128*16)
        jge     FFNANR                  | J/ return NaN if ABS(arg) >= 2^16
|
        movel   FIN2PI,sp@-     | Scale arg by 1/(2*PI)
        jsr     FPMUL
|
        cmpiw   #128*FBIAS,sp@
        jlt     FTS10           | /* J/ no excess two PI's */
|
        movel   sp@,sp@-        | Double the scaled argument
        jsr     INT
        negl    d0
        jsr     FLOAT
        jsr     FPADD
|
FTS10:
        tstw    sp@
        jeq     FTS20
|
        addiw   #128*3,sp@      | Multiply value by 8
        cmpiw   #128*FBIAS,sp@
        jlt     FTS20           | J/ < 1
|
        movel   sp@,sp@-        | Double parameter
        jsr     INT
        addqb   #1,d0
        lsrb    #1,d0
        addb    d0,a7@(4)       | Mix quadrant number with sign bit
|
        lslb    #1,d0
        negl    d0
        jsr     FLOAT
        jsr     FPADD
|
FTS20:
        tstw    sp@
        jeq     FTS30           | J/ arg = 0.0
|
        subiw   #128*1,sp@      | Range result to -0.5 to 0.5
|
FTS30:
        andib   #0x83,a7@(4)    | Limit quadrant shift to 0 thru 3
        moveb   a7@(4),d0       | Return cntl byte in d0 and at a7@(4)
        jmp     a6@
/*
|
| ###   SUBTTL  Polynomial Evaluation Routine
|       page
|
|  Polynomial Evaluation Routines.
|
|  A list of constants is pointed to by A6.  X is on the stack.  D0
|  contains the number of constants (the polynomial degree plus one).
|
|  A6 enters pointing to C[1].  Upon return, the value on stack has
|  been replaced with:
|  FXSER   = C[1]*X^(N-1)  + C[2]*X^(N-2)  + ... + C[N-1]*X   + C[N]
|
|  FX2SER  = C[1]*X^(2N-2) + C[2]*X^(2N-4) + ... + C[N-1]*X^2 + C[N]
|
|
*/

FX2SER:
        moveal  a4,a5           | Save a4 in a5
        bsr     FPFADJ
|
        moveb   d0,a7@(4)       | Save count
|
        movel   sp@,sp@-        | Square the argument
        jsr     FPMUL
        jra     FXSR01          | J/ join FXSER routine
|
FXSER:
        moveal  a4,a5
        bsr     FPFADJ
|
        moveb   d0,a7@(4)
|
FXSR01:
        movel   a6@+,sp@-       | Place C[1] on stack as accum
|
        subqb   #1,a7@(8)
|
FXSR02:
        movel   a7@(4),sp@-     | Copy X
        jsr     FPMUL
        movel   a6@+,sp@-       | Add in next coefficient
        jsr     FPADD
|
        subqb   #1,a7@(8)
        jne     FXSR02          | J/ more coefficients
|
        movel   sp@,a7@(8)      | Adjust return
        addql   #8,sp
|
        moveal  a4,a3
        moveal  a5,a4           | Restore return address
        jmp     a3@
/*
|
| ###   SUBTTL  Function Completion Segments
|       page
|
|  FPFADJ - Adjust the stack for single precision functions.
|          Move the return address to the function caller into A4.
|          Shift the single precision floating point argument down
|          four bytes, leaving a temporary area of four bytes at
|          4(A7) thru 7(A7).  Set the condition code bits N, Z,
|          C (INF), and V (NaN) to quickly type the argument.
|
*/

FPFADJ:
        moveal  sp@+,a3 | Function return addr
        moveal  sp@,a4          | Caller return addr
        movel   a7@(4),sp@      | Down shift parameter
|
        movew   sp@,d2          | Get SEM word
        rolw    #1,d2
        cmpiw   #256*0xFF,d2
        jcs     FPFA10          | J/ not INF or NaN
|
        andiw   #0xFE,d2
        jeq     FPFA05
|
        orib    #0x02,ccr       | Set -V- bit -> NaN argument
| ##    DC.W    $003C,$0002
        jmp     a3@
|
FPFA05:
        tstb    sp@             | /* Set N bit as req'd, clear Z bit. */
        orib    #0x01,ccr       | Set -C- bit -> INF argument
| ##    DC.W    $003C,$0001
        jmp     a3@
|
FPFA10:
        tstw    sp@             | /* Set N, Z bits as req'd, clear V, C */
        jmp     a3@
|
|
|
FFPINR:
        |dsw    0               | Result is positive overflow
        moveq   #0,d0           | Sign is positive
        jra     FFIN01
|
FFMINR:
        |dsw    0               | Result in negative overflow
        moveq   #-1,d0          | Sign is negative
|
FFIN01:
        moveaw  d0,a2           | Sign into a2
        addql   #8,sp           | Delete eight bytes from the stack
        moveal  a4,a0           | Return addr in a0
        jmp     FINFRS          | (Use FPOPNS exception return)
|
|
FFNANR:
        |dsw    0               | Result is NaN
        addql   #8,sp           | Delete eight bytes from the stack
        moveal  a4,a0
        jmp     FNANRS
|
|
FFZERR:
        |dsw    0               | Result is zero
        addql   #8,sp
        moveal  a4,a0
        jmp     FZERRS
|
|
FFUNFR:
        |dsw    0               | Underflow result
        addql   #8,sp           | Delete eight bytes from the stack
        moveal  a4,a0           | Return address into a0
        jmp     FUNFRS
|
|
FFONER:
        |dsw    0               | Result is 1.0
        addql   #4,sp
        movel   #0x3F800000,sp@
        clrb    FPERR
        jmp     a4@
|
|
|       end