* Extended arithmetic and floating point library

   incl inc:module.asm

   module arith
   export mul,external
   export div,external
   export muldiv,external
   export power,external
   export fneg,external
   export fadd,external
   export fsub,external
   export fmul,external
   export fdiv,external
   export fpow,external
   export float,external
   export round,external
   export trunc,external
   export fraction,external
   export int,external
   export intpt,external
   export fracpt,external
   export sqrt,external

   code

fb equ $3f00
plugin macro
\*     lea \1,a0
       move.l a0,fb+2-\2
       endm

reset nop
   plugin mul,12
   plugin div,18
   plugin muldiv,24
   plugin power,30
   plugin fneg,36
   plugin fadd,42
   plugin fsub,48
   plugin fmul,54
   plugin fdiv,60
   plugin fpow,66
   plugin float,72
   plugin round,78
   plugin trunc,84
   plugin fraction,90
   plugin int,96
   plugin intpt,102
   plugin fracpt,108
   plugin sqrt,114
begin rts

* 32x32 bit integer signed multiply to 32 bit result
* D0 := D0 * D1
* Overflow is reflected in SR on exit,
* but full mutiplication is done anyway.

mul movem.l d1-d4,-(sp)
   moveq #0,d4
   move.l d0,d2
   bpl.s m1
   neg.l d0
   moveq #1,d4
   move.l d0,d2
m1 swap d2
   move.l d1,d3
   bpl.s m2
   neg.l d1
   eor.w #1,d4
   move.l d1,d3
m2 swap d3
   tst.w d2
   beq.s maz     * A>>16=0 ->
   tst.w d3
   beq.s mbz     * B>>16=0 ->
   mulu d0,d3
   mulu d1,d2
   mulu d1,d0
   add.l d3,d2
   swap d0
   add.w d2,d0
   swap d0
   bra.s mv      * mandatory overflow ->
maz tst.w d3
   beq.s mabz    * A>>16=B>>16=0 ->
   mulu d0,d3
   mulu d1,d0
   swap d0
   add.w d3,d0
   swap d0
   swap d3
   tst.w d3
   bne.s mv      * certain overflow ->
   bra.s mvt     * possible overflow ->
mbz mulu d1,d2
   mulu d1,d0
   swap d0
   add.w d2,d0
   swap d0
   swap d2
   tst.w d2
   bne.s mv
   bra.s mvt
mabz mulu d1,d0
mvt tst.l d0
   bmi.s mv
   btst #0,d4
   beq.s m8
   neg.l d0
m8 tst.l d0
   movem.l (sp)+,d1-d4
   rts
mv btst #0,d4
   beq.s m9
   neg.l d0
m9 tst.l d0
   movem.l (sp)+,d1-d4
   or.b #2,ccr
   rts

* Signed 32 bit integer muldiv using 64 bit product
* D0 := (D0 * D1) // D2,  D1 := remainder

mod macro
   move.l \1,\2
   bpl.s \@
   neg.l \1
   neg.l \2
\@ equ *
   endm

muldiv movem.l d0-d5,-(sp)
   mod d0,d2
   swap d2
   mod d1,d3
   swap d3
   move.w d3,d4
   mulu d2,d4    * A*B
   mulu d0,d3    * a*B
   mulu d1,d2    * A*b
   mulu d1,d0    * a*b
   move.l d4,d1
   add.l d2,d3
   bcc.s md1
   add.l #$10000,d1
md1 swap d0
   add.w d3,d0
   bcc.s md2
   addq.l #1,d1
md2 swap d0
   clr.w d3
   swap d3
   add.l d3,d1      * D1_D0 = A_a*B_b
   clr.l d2
   move.l 8(sp),d3  * D3_D2 = Cc<<32
   bpl.s md3
   neg.l d3
md3 move.l #$80000000,d4
   clr.l d5         * quotient
   cmp.l d3,d1
   bhs.s mdv        * overflow ->
md4 lsr.l #1,d3
   roxr.l d2
   cmp.l d3,d1
   bhi.s mds
   blo.s mdns
   cmp.l d2,d0
   blo.s mdns
mds sub.l d2,d0
   subx.l d3,d1
   add.l d4,d5
mdns lsr.l #1,d4
   bcc.s md4
   move.l d0,d1  * remainder
   move.l d5,d0  * quotient
   move.w (sp),d2
   eor.w d2,4(sp)
   bpl.s md8     * non-neg rem ->
   neg.l d1
md8 move.w 8(sp),d2
   eor.w d2,4(sp)
   bpl.s md9     * non-neg quot ->
   neg.l d0
md9 addq.l #8,sp
   movem.l (sp)+,d2-d5
   tst.l d0
   rts
mdv movem.l (sp)+,d0-d5
overflow or.b #2,ccr
   trapv
   rts

* Divide 32 bit integer D0 by 32 bit D1
* D0 := quot, D1 := rem

div      movem.l d2-d4,-(sp)
         clr     d4
         move.l  d1,d2
         beq.s   zdivoflow
         bpl.s   divpos1
         neg.l   d2
         or      #2,d4
divpos1  move.l  d0,d1
         bpl.s   divpos2
         neg.l   d1
         eor.l   #3,d4
divpos2  clr.l   d0
         move.l  #1,d3
         bra.s   divsulps
divsulp  lsl.l   #1,d2
         lsl.l   #1,d3
divsulps cmp.l   d2,d1
         bhi.s   divsulp
divlp    cmp.l   d2,d1
         bcs.s   divnosub
         add.l   d3,d0
         sub.l   d2,d1
divnosub lsr.l   #1,d2
         lsr.l   #1,d3
         bne     divlp
         lsr     #1,d4
         bcc.s   nonegrem
         neg.l   d1
nonegrem tst     d4
         beq.s   nonegquo
         neg.l   d0
nonegquo tst.l   d0
         movem.l (sp)+,d2-d4
         rts
zdivoflow movem.l (sp)+,d2-d4
         divs #0,d0            * invoke trap
         rts                   * never reached

************************************
*                                  *
*     Floating Point Package       *
*                                  *
* Copyright M. R. King 1982, 1984. *
* All rights reserved.             *
*                                  *
************************************

         dc.b     'Floating Point Package. '
         dc.b     'Copyright M.R.King, 1982, 1984. '
         dc.b     'All rights reserved.'
         ds.w     0

float    movem.l  d1,-(sp)
         move.w   #150*128,d1
         tst.l    d0
         beq.s    floatret
         bpl.s    floatpos
         eor.w    #$8000,d1
         neg.l    d0
floatpos cmp.l    #$00800000,d0
         bcc.s    floatsr
floatll  lsl.l    d0
         sub.w    #128,d1
         btst.l   #23,d0
         beq.s    floatll
         bra.s    floatex
floatrl  lsr.l    d0
         add.w    #128,d1
floatsr  cmp.l    #$00FFFFFF,d0
         bcc.s    floatrl
floatex  swap     d0
         and.w    #$007F,d0
         or.w     d1,d0
         swap     d0
floatret movem.l  (sp)+,d1
         rts

fneg     tst.l    d0
         beq.s    fpnegret
         eor.l    #$80000000,d0
fpnegret rts

round    tst.l    d0
         bpl.s    int
         eor.l    #$80000000,d0
         bsr.s    int
         neg.l    d0
         rts

int      move.l   d1,-(sp)
         move.l   #$3f000000,d1
         bsr      fadd
         move.l   (sp)+,d1

intpt    movem.l  d1-d2,-(sp)
         swap     d0
         move.w   d0,d1
         and.w    #$7F,d0
         or.w     #$80,d0
         swap     d0
         move.w   d1,d2
         lsr.w    #7,d1
         and.w    #$FF,d1
         sub.w    #150,d1
         bcc.s    intptsl
         neg.w    d1
         cmp.w    #24,d1
         bcc.s    intptzer
         tst.w    d2
         bpl.s    intptpos
         move.l   #-1,d2
         lsl.l    d1,d2
         not.l    d2
         and.l    d0,d2
         bne.s    intptni
         lsr.l    d1,d0
         neg.l    d0
         bra.s    intptret
intptni  lsr.l    d1,d0
         not.l    d0
         bra.s    intptret
intptsl  cmp.w    #9,d1
         bcc.s    intptofl
         tst.w    d2
         bpl.s    intptlp
         neg.l    d0
intptlp  asl.l    d1,d0
         bvc.s    intptret
intptofl movem.l  (sp)+,d1-d2
         bra      overflow
intptpos lsr.l    d1,d0
intptret movem.l  (sp)+,d1-d2
         rts
intptzer tst.w    d2
         movem.l  (sp)+,d1-d2
         bpl.s    intptzp
         tst.l    d0
         beq.s    intptzr
         move.l   #-1,d0
         rts
intptzp  move.l   #0,d0
intptzr  rts

trunc    movem.l  d1-d2,-(sp)
         swap     d0
         move.w   d0,d1
         and.w    #$7F,d0
         or.w     #$80,d0
         swap     d0
         move.w   d1,d2
         lsr.w    #7,d1
         and.w    #$FF,d1
         sub.w    #150,d1
         bcc.s    truncsl
         neg.w    d1
         cmp.w    #24,d1
         bcc.s    trunczer
         lsr.l    d1,d0
         tst.w    d2
         bpl.s    truncst
         neg.l    d0
         bra.s    truncret
truncsl  cmp.w    #9,d1
         bcc.s    truncofl
         tst.w    d2
         bpl.s    trunclp
         neg.l    d0
trunclp  asl.l    d1,d0
         bvc.s    truncret
truncofl movem.l  (sp)+,d1-d2
         bra      overflow
truncst  tst.l    d0
truncret movem.l  (sp)+,d1-d2
         rts
trunczer clr.l    d0
         bra.s    truncret

fdiv     movem.l  d1-d4,-(sp)
         clr.w    d4
         tst.l    d1
         bgt.s    fdiv1pos
         eor.w    #$0100,d4
         and.l    #$7FFFFFFF,d1
         bne.s    fdiv1pos
         movem.l  (sp)+,d1-d4
         bra      overflow
fdiv1pos tst.l    d0
         bgt.s    fdiv0pos
         and.l    #$7FFFFFFF,d0
         beq.s    fdivret
         eor.w    #$0100,d4
fdiv0pos bsr      fpextrct
         add.w    #151,d2
         sub.w    d3,d2
         bcs.s    fdivzero
         clr.l    d3
fpdivlp  sub.w    #1,d2
         bcs.s    fdivzero
         lsl.l    d3
         cmp.l    d1,d0
         bcs.s    fdvnosub
         sub.l    d1,d0
         add.l    #1,d3
fdvnosub lsl.l    d0
         btst.l   #23,d3
         beq.s    fpdivlp
         cmp.w    #256,d2
         bcs.s    fdivnofl
         movem.l  (sp)+,d1-d4
         bra      overflow
fdivnofl or.w     d4,d2
         lsl.w    #7,d2
         swap     d3
         and.w    #$007F,d3
         or.w     d2,d3
         swap     d3
         move.l   d3,d0
fdivret  movem.l  (sp)+,d1-d4
         rts
fdivzero clr.l    d0
         bra.s    fdivret

fmul     movem.l  d1-d6,-(sp)
         clr.w    d6
         tst.l    d0
         bgt.s    fmul0pos
         and.l    #$7FFFFFFF,d0
         beq      fmulret
         eor.w    #$0100,d6
fmul0pos tst.l    d1
         bgt.s    fmul1pos
         and.l    #$7FFFFFFF,d1
         beq.s    fmulzero
         eor.w    #$0100,d6
fmul1pos bsr      fpextrct
         add.w    d3,d2
         sub.w    #126,d2
         bcs.s    fmulzero
         move.l   d0,d3
         move.l   d1,d4
         mulu     d0,d1
         swap     d0
         swap     d4
         mulu     d4,d0
         swap     d1
         move.w   d4,d5
         mulu     d3,d5
         add.w    d5,d1
         clr.w    d5
         swap     d5
         addx.l   d5,d0
         swap     d3
         swap     d4
         mulu     d4,d3
         add.w    d3,d1
         clr.w    d3
         swap     d3
         addx.l   d3,d0
         lsl.l    #8,d0
         rol.w    #8,d1
         move.b   d1,d0
         lsl.w    d1
         btst.l   #23,d0
         bne.s    fmnoshft
         roxl.l   d0
         sub.w    #1,d2
         bcs.s    fmulzero
         lsl.w    d1
fmnoshft clr.l    d1
         addx.l   d1,d0
         btst.l   #24,d0
         beq.s    fmnocarr
         lsr.l    d0
         add.w    #1,d2
fmnocarr cmp.w    #256,d2
         bcs.s    fmulnofl
         movem.l  (sp)+,d1-d6
         bra      overflow
fmulzero clr.l    d0
         bra.s    fmulret
fmulnofl swap     d0
         and.w    #$007F,d0
         or.w     d6,d2
         lsl.w    #7,d2
         or.w     d2,d0
         swap     d0
fmulret  movem.l  (sp)+,d1-d6
         rts

fpextrct swap    d0
         move.w  d0,d2
         and.w   #$007F,d0
         or.w    #$0080,d0
         swap    d0
         lsr.w   #7,d2
         swap    d1
         move.w  d1,d3
         and.w   #$007F,d1
         or.w    #$0080,d1
         swap    d1
         lsr     #7,d3
         rts

fadd     movem.l  d1-d4,-(sp)
         clr.w    d4
         tst.l    d0
         bne.s    fadd0nz
         move.l   d1,d0
         bra      fpasret
fadd0nz  bpl.s    fadd0pos
         and.l    #$7FFFFFFF,d0
         exg      d0,d1
         bra.s    fsub1pos
fadd0pos tst.l    d1
         beq      fpasret
         bpl.s    fadd1pos
         and.l    #$7FFFFFFF,d1
         bra.s    fsub0pos
fadd1pos cmp.l    d1,d0
         bcc.s    fadd0big
         exg      d0,d1
fadd0big bsr      fpextrct
         sub.w    d3,d2
         add.w    d2,d3
         cmp.w    #24,d2
         bcc      fasquick
         lsr.l    d2,d1
         add.l    d1,d0
         btst.l   #24,d0
         beq      fasquick
         add.l    #1,d0
         lsr.l    #1,d0
         add.b    #1,d3
         bcc.s    fasquick
         movem.l  (sp)+,d1-d4
         bra      overflow

fsub     movem.l  d1-d4,-(sp)
         clr.w    d4
         tst.l    d1
         beq.s    fpasret
         bpl.s    fsub1pos
         and.l    #$7FFFFFFF,d1
         exg      d0,d1
         bra.s    fadd0pos
fsub1pos tst.l    d0
         bne.s    fsub0nz
         move.l   d1,d0
         eor.l    #$80000000,d0
         bra.s    fpasret
fsub0nz  bpl.s    fsub0pos
         and.l    #$7FFFFFFF,d0
         eor.w    #$0100,d4
         bra.s    fadd1pos
fsub0pos cmp.l    d1,d0
         bcc.s    fsub0big
         exg      d0,d1
         eor.w    #$0100,d4
fsub0big bsr      fpextrct
         sub.w    d3,d2
         add.w    d2,d3
         cmp.w    #24,d2
         bcc.s    fasquick
         lsr.l    d2,d1
         sub.l    d1,d0
         beq.s    fpasret
         bhi.s    fsubsrch
         neg.l    d0
         eor.w    #$0100,d4
         bra      fsubsrch
fsbsrchl sub.b    #1,d3
         bcs.s    fsubzero
         lsl.l    d0
fsubsrch btst.l   #23,d0
         beq.s    fsbsrchl
fasquick or.w     d3,d4
         lsl.w    #7,d4
         swap     d0
         and.w    #$007F,d0
         or.w     d4,d0
         swap     d0
fpasret  tst.l    d0
         bpl.s    fpasretp
         and.l    #$7FFFFFFF,d0
         beq.s    fpasretp
         eor.l    #$80000000,d0
fpasretp movem.l  (sp)+,d1-d4
         rts
fsubzero clr.l    d0
         bra.s    fpasretp

fracpt   movem.l  d1-d2,-(sp)
         swap     d0
         move.w   d0,d1
         bpl.s    fractpen
         and.w    #$7F,d0
         or.w     #$80,d0
         swap     d0
         lsr.w    #7,d1
         and.w    #$FF,d1
         sub.w    #126,d1
         blo.s    fracptal
         cmp.w    #24,d1
         bcc.s    fracptzr
         lsl.l    d1,d0
         move.w   #126,d1
         neg.l    d0
         and.l    #$00FFFFFF,d0
         bne.s    fracptsc
         bra.s    fracptzr
fracptsl sub.w    #1,d1
         bcs.s    fracptzr
         lsl.l    #1,d0
fracptsc btst.l   #23,d0
         beq.s    fracptsl
         bra.s    fracptex
fracptal neg.w    d1
         lsr.l    d1,d0
         sub.l    #$1000000,d0
         beq.s    fracptzr
         neg.l    d0
         move.w   #126,d1
fracptex lsl.w    #7,d1
         swap     d0
         and.w    #$007F,d0
         or.w     d1,d0
         swap     d0
fracptrt movem.l  (sp)+,d1-d2
         rts
fracptzr clr.l    d0
         bra.s    fracptrt

fraction movem.l  d1-d2,-(sp)
         swap     d0
         move.w   d0,d1
fractpen and.w    #$7F,d0
         swap     d0
         lsr.w    #7,d1
         move.w   d1,d2
         and.w    #$FF,d1
         and.w    #$0100,d2
         sub.w    #126,d1
         bls.s    fractal
         cmp.w    #24,d1
         bcc.s    fractzr
         lsl.l    d1,d0
         move.w   #126,d1
         and.l    #$00FFFFFF,d0
         bne.s    fractsc
         bra.s    fractzr
fractsl  sub.w    #1,d1
         bcs.s    fractzr
         lsl.l    #1,d0
fractsc  btst.l   #23,d0
         beq.s    fractsl
         bra.s    fractex
fractal  add.w    #126,d1
fractex  or.w     d2,d1
         lsl.w    #7,d1
         swap     d0
         and.w    #$007F,d0
         or.w     d1,d0
         swap     d0
fractrt  movem.l  (sp)+,d1-d2
         rts
fractzr  clr.l    d0
         bra.s    fractrt

fpow     movem.l d1-d3,-(sp)
         move.l d1,d3
         beq.s rpzer
         bpl.s rppos
         neg.l d3
         move.l d0,d1
         move.l #$3f800000,d0
         bsr fdiv
rppos    move.l #$3f800000,d2
         move.l d0,d1
rploop   btst #0,d3
         beq.s rpno
         move.l d2,d0
         bsr fmul
         move.l d0,d2
rpno     lsr.l #1,d3
         beq.s rpret
         move.l d1,d0
         bsr fmul
         move.l d0,d1
         bra rploop
rpret    move.l d2,d0
         movem.l (sp)+,d1-d3
         rts
rpzer    move.l #$3f800000,d2
         bra rpret

power    movem.l d1-d3,-(sp)
         move.l d1,d3
         beq.s intpzer
         bmi.s intpneg
         moveq #1,d2
         move.l d0,d1
intploop btst #0,d3
         beq.s intpno
         move.l d2,d0
         jsr mul
         trapv
         move.l d0,d2
intpno   lsr.l #1,d3
         beq.s intpret
         move.l d1,d0
         jsr mul
         trapv
         move.l d0,d1
         bra intploop
intpret  move.l d2,d0
         movem.l (sp)+,d1-d3
         rts
intpzer  moveq #1,d2
         bra intpret
intpneg  clr.l d2
         bra intpret

sqrt     move.w   ccr,-(sp)
         and.l    #$7FFFFFFF,d0
         beq.s    psqrtret
         movem.l  d1-d4,-(sp)
         swap     d0
         move.w   d0,d1
         and.w    #$007F,d0
         or.w     #$0080,d0
         swap     d0
         lsl.l    #7,d0
         sub.w    #$3F80,d1
         asr.w    #1,d1
         add.w    #$3F00,d1
         btst.l   #6,d1
         beq.s    fpsqrtns
         lsl.l    #1,d0
fpsqrtns move.l   #0,d2
         move.l   #0,d3
         move.l   #23,d4
fpsqrtlp lsl.l    #1,d0
         roxl.l   #1,d2
         lsl.l    #1,d0
         roxl.l   #1,d2
         lsl.l    #1,d3
         add.l    #1,d3
         sub.l    d3,d2
         bcs.s    fpsqrtob
         add.l    #1,d3
         dbra     d4,fpsqrtlp
         bra.s    fpsqrtnm
fpsqrtob add.l    d3,d2
         sub.l    #1,d3
         dbra     d4,fpsqrtlp
fpsqrtnm add.w    #$0080,d1
         add.l    #1,d3
         lsr.l    #1,d3
         btst.l   #24,d3
         bne.s    fpsqrtnm
         and.w    #$7F80,d1
         move.w   d1,d0
         swap     d3
         and.w    #$007F,d3
         or.l     d3,d0
         movem.l  (sp)+,d1-d4
psqrtret move.w   (sp)+,ccr
         swap     d0
         rts

         endmodule

         end
