.mmregs
      
      .global _L_Extract
      .global _L_Comp
      .global _Mpy_32
      .global _Mpy_32_16
      .global _Div_32
      
      .ref    _Overflow
      .ref    _Error
      
      .text
      
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;void L_Extract(Word32 L_32, Word16 *hi, Word16 *lo)
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
_L_Extract:
    PSHM    ST0
    PSHM    ST1
    RSBX    OVA
    RSBX    OVB

    ssbx	sxm
    ssbx    ovm
    
    mvdk    *sp(3),ar3
    mvdk    *sp(4),ar4
    
    sth     a,*ar3
    ld      a,-1,a
    sub     *ar3,15,a
    stl     a,*ar4
    
    POPM    ST1
    POPM    ST0
    RET
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;Word32 L_Comp(Word16 hi, Word16 lo)
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
_L_Comp:
    PSHM    ST0
    PSHM    ST1
    RSBX    OVA
    RSBX    OVB

    ssbx	sxm
    ssbx    ovm   
    
    ld      a,15,a
    ld      a,1,a
    
    add     *sp(3),1,a
        
    POPM    ST1
    POPM    ST0
    ret
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;Word32 Mpy_32(Word16 hi1, Word16 lo1, Word16 hi2, Word16 lo2)
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
_Mpy_32:
    PSHM    ST0
    PSHM    ST1
    RSBX    OVA
    RSBX    OVB

    ssbx	sxm
    ssbx    ovm
    ssbx    frct
    
    st      #0,*(_Overflow)
    stlm    a,t
    mpy     *sp(4),b
    
    mpy     *sp(5),a
    ld      a,-15,a
    add     a,b
    
    ld      *sp(3),t
    mpy     *sp(4),a
    ld      a,-15,a
    add     a,b
    nop
    nop
    xc      2,bov
    st      #1,*(_Overflow)

    ld      b,a       
    
    POPM    ST1
    POPM    ST0
    ret 
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;Word32 Mpy_32_16(Word16 hi, Word16 lo, Word16 n)
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
_Mpy_32_16:
    PSHM    ST0
    PSHM    ST1
    RSBX    OVA
    RSBX    OVB

    ssbx	sxm
    ssbx    ovm
    ssbx    frct
    
    st      #0,*(_Overflow)
    stlm    a,t
    mpy     *sp(4),b
    
    ld      *sp(3),t
    mpy     *sp(4),a
    
    add     a,-15,b
    nop
    nop
    xc      2,bov
    st      #1,*(_Overflow)
    
    ld      b,a
    
    POPM    ST1
    POPM    ST0
    ret 
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;Word32 Div_32(Word32 L_num, Word16 denom_hi, Word16 denom_lo)
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
_Div_32:
    PSHM    ST0
    PSHM    ST1
    RSBX    OVA
    RSBX    OVB

    frame   -4
    ssbx    sxm
    ssbx    ovm
    ssbx    frct

    st      #0,*(_Error)
    st      #0,*(_Overflow)

    bc      Div_32_Er1,alt
    ld      *sp(7),b
    bc      Div_32_Er1,bleq
    ld      #3fffh,b
    sub     *sp(7),b
    bc      Div_32_P,bleq
Div_32_Er1:
    st      #1,*(_Error)
    b       Div_32_E
Div_32_P:
    sth     a,*sp(0)
    sub     *sp(0),16,a
    stl     a,-1,*sp(1)         ;L_num分為n_hi,n_lo
;First approximation: 1 / L_denom = 1/denom_hi    
    ld      #3fffh,16,b
    rpt     #14
    subc    *sp(7),b
    and     #7fffh,b            ;1/demon_hi,result in Q14,Note: 3fff = 0.5 in Q15
;1/L_denom = approx * (2.0 - L_denom * approx)    
    stlm    b,t
    mpy     *sp(7),b
    mpy     *sp(8),a
    add     a,-15,b              ;L_denom * approx,result in Q30
    
    ld      #7fffh,16,a         
    or      #0ffffh,a
    sub     b,a                  ;2.0 - L_denom * approx,result in Q30
    
    sth     a,*sp(2)
    sub     *sp(2),16,a
    stl     a,-1,*sp(3)          ;L_Extract(L_32, &hi, &lo)
    
    mpy     *sp(2),b
    mpy     *sp(3),a
    add     a,-15,b              ;Mpy_32_16(hi, lo, approx)in Q29
; L_num * (1/L_denom)    
    sth     b,*sp(2)
    sub     *sp(2),16,b
    stl     b,-1,*sp(3)          ;L_Extract(L_32, &hi, &lo)
    
    ld      *sp(0),t
    mpy     *sp(2),b
    mpy     *sp(3),a
    add     a,-15,b
    ld      *sp(1),t
    mpy     *sp(2),a
    add     a,-15,b              ;L_32 = Mpy_32(n_hi, n_lo, hi, lo),result in Q29
    
    ld      b,2,b                ;From Q29 to Q31
    
    nop
    nop
    xc      2,bov
    st      #1,*(_Overflow)
    
    ld      b,a
Div_32_E: 
    frame   4   
    POPM    ST1
    POPM    ST0
    RET
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
    .end