millfork/include/zp_reg.mfk

#if not(ARCH_6502)
#warn zp_reg module should be used only on 6502-compatible targets
#endif

noinline asm byte __mul_u8u8u8() {
    ? LDA #0
    ? JMP __mul_u8u8u8_start
__mul_u8u8u8_add:
    ? CLC
    ? ADC __reg
__mul_u8u8u8_loop:
    ? ASL __reg
__mul_u8u8u8_start:
    ? LSR __reg+1
    ? BCS __mul_u8u8u8_add
    ? BNE __mul_u8u8u8_loop
    ? RTS
}

// divide __reg[0]/__reg[1]

noinline asm byte __mod_u8u8u8u8() {
    ? LDA #0
    ? LDX #7
    ? CLC
__divmod_u8u8u8u8_start:
    ? ROL __reg
    ? ROL
    ? CMP __reg+1
    ? BCC __divmod_u8u8u8u8_skip
    ? SBC __reg+1
__divmod_u8u8u8u8_skip:
    ? DEX
    ? BPL __divmod_u8u8u8u8_start
    ? ROL __reg
    ? RTS
}

asm byte __div_u8u8u8u8() {
    ? JSR __mod_u8u8u8u8
    ? LDA __reg
    ? RTS
}

#if ZPREG_SIZE >= 3

noinline asm word __mul_u16u8u16() {
    ? LDA #0
    ? TAX
    ? JMP __mul_u16u8u16_start
__mul_u16u8u16_add:
    ? CLC
    ? ADC __reg
    ? TAY
    ? TXA
    ? ADC __reg+1
    ? TAX
    ? TYA
__mul_u16u8u16_loop:
    ? ASL __reg
    ? ROL __reg+1
__mul_u16u8u16_start:
    ? LSR __reg+2
    ? BCS __mul_u16u8u16_add
    ? BNE __mul_u16u8u16_loop
    ? RTS
}

// divide (__reg[1]:__reg[0])/__reg[2]
// remainder in A, quotient in (__reg[1]:__reg[0])

noinline asm byte __mod_u16u8u16u8() {
    ? LDX #15
    ? CLC
// TODO: decide when
#if OPTIMIZE_FOR_SPEED
    ? LDA __reg+2
    ? BMI __divmod_u16u8u16u8_variant2
    ? LDA #0
__divmod_u16u8u16u8_start: // for divisors <= 127
    ? ROL __reg
    ? ROL __reg+1
    ? ROL
    ? CMP __reg+2
    ? BCC __divmod_u16u8u16u8_skip
    ? SBC __reg+2
__divmod_u16u8u16u8_skip:
    ? DEX
    ? BPL __divmod_u16u8u16u8_start
    ? ROL __reg
    ? ROL __reg+1
    ? RTS
#endif
__divmod_u16u8u16u8_variant2:
    ? LDA #0
__divmod_u16u8u16u8_start2: // for divisors >= 128
    ? ROL __reg
    ? ROL __reg+1
    ? ROL
    ? BCS __divmod_u16u8u16u8_sub2
    ? CMP __reg+2
    ? BCC __divmod_u16u8u16u8_skip2
__divmod_u16u8u16u8_sub2:
    ? SBC __reg+2
    ? SEC
__divmod_u16u8u16u8_skip2:
    ? DEX
    ? BPL __divmod_u16u8u16u8_start2
    ? ROL __reg
    ? ROL __reg+1
    ? RTS
}

asm word __div_u16u8u16u8() {
    ? JSR __mod_u16u8u16u8
    ? LDA __reg
    ? LDX __reg+1
    ? RTS

}

#endif

#if ZPREG_SIZE >= 4

noinline asm word call(word ax) {
    JMP ((__reg + 2))
}

noinline asm word __mul_u16u16u16() {
#if ZPREG_SIZE < 6
    ? LDA __reg+1
    ? PHA
    ? LDA __reg
    ? PHA
    ? TSX
#else
    ? LDA __reg
    ? STA __reg+4
    ? LDA __reg+1
    ? STA __reg+5
#endif
#if CPUFEATURE_65C02 && not(CPUFEATURE_65CE02)
    ? STZ __reg
    ? STZ __reg+1
#else
    ? LDA #0
    ? STA __reg
    ? STA __reg+1
#endif
    ? LDY #16
__mul_u16u16u32_loop:
    ? ASL __reg
    ? ROL __reg+1
    ? ROL __reg+2
    ? ROL __reg+3
    ? BCC __mul_u16u16u32_skip
#if ZPREG_SIZE < 6
    ? LDA  $101,X
#else
    ? LDA  __reg+4
#endif
    ? CLC
    ? ADC __reg
    ? STA __reg
#if ZPREG_SIZE < 6
    ? LDA  $102,X
#else
    ? LDA  __reg+5
#endif
    ? ADC __reg+1
    ? STA __reg+1
    ? BCC __mul_u16u16u32_skip
    ? INC __reg+2
__mul_u16u16u32_skip:
    ? DEY
    ? BNE __mul_u16u16u32_loop
#if ZPREG_SIZE < 6
#if OPTIMIZE_FOR_SPEED
    ? INX
    ? INX
    ? TXS
#else
    ? PLA
    ? PLA
#endif
#endif
    ? LDA __reg
    ? LDX __reg+1
    ? RTS
}

// divide (__reg[1]:__reg[0])/(__reg[3]:__reg[2])
// remainder in (__reg[2]:__reg[3]), quotient in (__reg[1]:__reg[0])

noinline asm word __divmod_u16u16u16u16() {
#if ZPREG_SIZE < 6
    ? LDA __reg+3
    ? PHA
    ? LDA __reg+2
    ? PHA
#else
    ? LDA __reg+2
    ? STA __reg+4
    ? LDA __reg+3
    ? STA __reg+5
#endif
#if CPUFEATURE_65C02 && not(CPUFEATURE_65CE02)
    ? STZ __reg+2
    ? STZ __reg+3
#else
    ? LDA #0
    ? STA __reg+2
    ? STA __reg+3
#endif
#if ZPREG_SIZE < 6
    ? LDA #16
    ? PHA
    ? TSX
#else
    ? LDX #16
#endif
__divmod_u16u16u16u16_loop:
    ? ASL __reg
	? ROL __reg+1
	? ROL __reg+2
	? ROL __reg+3
	? LDA __reg+2
	sec
#if ZPREG_SIZE < 6
	? SBC $102,X
#else
	? SBC __reg+4
#endif
	? TAY
	? LDA __reg+3
#if ZPREG_SIZE < 6
	? SBC $103,X
#else
	? SBC __reg+5
#endif
	? BCC __divmod_u16u16u16u16_skip
	? STA __reg+3
	? STY __reg+2
	? INC __reg
__divmod_u16u16u16u16_skip:
#if ZPREG_SIZE < 6
    ? DEC $101,X
#else
    ? DEX
#endif
	? BNE __divmod_u16u16u16u16_loop
#if ZPREG_SIZE < 6
#if OPTIMIZE_FOR_SIZE
    ? PLA
    ? PLA
    ? PLA
#else
    ? INX
    ? INX
    ? INX
    ? TXS
#endif
#endif
	? RTS
}

asm word __div_u16u16u16u16() {
    JSR __divmod_u16u16u16u16
    ? LDA __reg
    ? LDX __reg+1
    ? RTS
}

asm word __mod_u16u16u16u16() {
    JSR __divmod_u16u16u16u16
    ? LDA __reg+2
    ? LDX __reg+3
    ? RTS
}

#endif