*============================================================================*
*               1 6   B I T   M A T H   P A C K A G E
*          for the Motorola 6805 family of microprocessors.
*
*                            D G Weiss
*                          Motorola Inc
*                      Microprocessor Group
*                 Midrange Strategic Engineering
*                          13 Nov 1985
*
*   This package features maximum simplicity, with each routine accepting
*   operands and leaving results in fixed memory locations.
*
*============================================================================*
* Multiply 16 by 16 bit unsigned integer routine
*
*    Accepts:
*       Multiplier:    Accum[1..0]
*       Multiplicand:  Mpd[1..0]
*    Yields:
*       Product:       Accum[3..0]
*----------------------------------------------------------------------------*
        ORG  $40       Beginning of RAM
ACCUM3  RMB  1         Accumulator byte 3 (high order)
ACCUM2  RMB  1
ACCUM1  RMB  1
ACCUM0  RMB  1         Accumulator byte 0 (low order)
CTR     RMB  1         Iteration counter
*
PROD3   EQU  ACCUM3    Product byte 3 (high order)
PROD2   EQU  ACCUM2
PROD1   EQU  ACCUM1
PROD0   EQU  ACCUM0    Product byte 0 (low order)
MPR1    RMB  1         Multiplier byte 1 (high order)
MPR0    RMB  1         Multiplier byte 0 (low order)
MPD1    RMB  1         Multiplicand byte 1 (high order)
MPD0    RMB  1         Multiplicand byte 0 (low order)
*
RAM1    EQU  *       RAM marker for later RMBs
*
        ORG  $800

MPY32   EQU  *
        LDA  PROD0                 .Mpr[1..0] := Prod[1..0]
        STA  MPR0
        LDA  PROD1
        STA  MPR1
        CLR  PROD0                 .Prod[1..0] := 0
        CLR  PROD1
        LDA  #16                   .For ctr := 16 Downto 1 Do
        STA  CTR

MPY32A  BRCLR 7,MPR1,MPY32C        .   If mpr msb = 1
        LDA  PROD0                 .       Then prod += mpd
        ADD  MPD0
        STA  PROD0
        LDA  PROD1
        ADC  MPD1
        STA  PROD1
        LDA  PROD2
        ADC  #0
        STA  PROD2

MPY32C  DEC  CTR                   .   If (ctr -= 1) = 0 Then Exit
        BEQ  MPY32E
        ASL  PROD0                 .   shift prod left
        ROL  PROD1
        ROL  PROD2
        ROL  PROD3
        ASL  MPR0                  .   shift mpr left
        ROL  MPR1
        BRA  MPY32A

MPY32E  RTS
*
*----------------------------------------------------------------------------*
* Divide 32 by 16 bit unsigned integer routine
*
*    Accepts:
*       Dividend:      Accum[3..0]
*       Divisor:       Dvsor[1..0]
*    Yields:
*       Quotient:      Accum[1..0]
*       Remainder:     Accum[3..2]
*----------------------------------------------------------------------------*
        ORG  RAM1    get back to RAM area
DVDND3  EQU  ACCUM3  Dividend byte 3 (high order)
DVDND2  EQU  ACCUM2
DVDND1  EQU  ACCUM1
DVDND0  EQU  ACCUM0  Dividend byte 0 (low order)
RMNDR1  EQU  ACCUM3  Remainder byte 1 (high order)
RMNDR0  EQU  ACCUM2  Remainder byte 0 (low order)
QUOT1   EQU  ACCUM1  Quotient byte 1 (high order)
QUOT0   EQU  ACCUM0  Quotient byte 0 (low order)
DVSOR1  RMB  1       Divisor byte 1 (high order)
DVSOR0  RMB  1       Divisor byte 0 (low order)
*
        ORG  $900
DIV32   EQU  *
        LDA  #16                       .For Ctr := 16 Downto 1 do
        STA  CTR

DIV32A  LDA  DVDND3                    .    shift dividend right one place
        ROLA
        ROL  DVDND0
        ROL  DVDND1
        ROL  DVDND2
        ROL  DVDND3

        LDA  DVDND2                    .    subtract divisor from remainder
        SUB  DVSOR0
        STA  DVDND2
        LDA  DVDND3
        SBC  DVSOR1
        STA  DVDND3
        LDA  DVDND0                    .{dividend low bit holds subtract carry}
        SBC  #0
        STA  DVDND0

        BRCLR 0,DVDND0,DIV32C          .    if subtract carry = 1
        LDA  DVDND2                    .        Then add divisor back in
        ADD  DVSOR0
        STA  DVDND2
        LDA  DVDND3
        ADC  DVSOR1
        STA  DVDND3
        LDA  DVDND0
        ADC  #0
        STA  DVDND0
        BRA  DIV32D
DIV32C  BSET 0,DVDND0                  .        Else set hi bit := 1

DIV32D  DEC  CTR
        BNE  DIV32A
        RTS