Simple calculator implementation in 6502 assembly

calc6502.asm

; 6502 STACK BASED CALCULATOR.
; THE FIRST ARGUMENT MUST BE
; THE ID FOR THE OPERATION.
; 1 = ADDITION          | 2 = SUBTRACTION
; 3 = MULTIPLICATION    | 4 = DIVITION
; THE RETURN VALUE OF THE OPERATIONS
; IS STORED IN THE TOP OF THE STACK.
;
; *** NOTE: THIS IS NOT A FAST IMPLEMENTATION ***
JMP MAIN
CALC:           ; OPERATION PREPROCESS
    PLA         ; FP LOW BYTE
    STA $1000
    PLA         ; FP HIGH BYTE
    STA $1001
    PLA         ; OP TYPE
    CMP #$01 
    BEQ ADDOP   ; ADD SUBROUTINE
    CMP #$02
    BEQ SUBOP   ; SUB SUBROUTINE
    CMP #$03
    BEQ MULOP   ; MUL SUBROUTINE
    CMP #$04
    BEQ DIVOP   ; DIV SUBROUTINE
    BRK
ENDOP:          ; OPERATION POSTPROCESS
    TYA
    PHA         ; RETURN VALUE
    LDA $1001
    PHA
    LDA $1000   ; RESTORE FP
    PHA
    RTS
ADDOP:          ; ADD OPERATION
    PLA
    TAX         ; ARG 1
    PLA 
    TAY         ; ARG 0
ADDOP_0:
    DEX
    INY
    CPX #$00
    BNE ADDOP_0
    JMP ENDOP
SUBOP:          ; SUBTRACT OPERATION
    PLA
    TAX         ; ARG 1
    PLA 
    TAY         ; ARG 0
SUBOP_0:
    DEX
    DEY
    CPX #$00
    BNE SUBOP_0
    JMP ENDOP
MULOP:          ; MULTIPLY OPERATION
    PLA
    TAX         ; ARG 1
    PLA 
    TAY         ; ARG 0
    LDA #$00
    STA $1005
    STX $1004
MUL_0:
    INC $1005
    DEX
    CPX #$00
    BNE MUL_0
    LDX $1004
    DEY
    CPY #$00
    BNE MUL_0
    LDY $1005
    JMP ENDOP
DIVOP:          ; DIVIDE OPERATION
    PLA
    TAX         ; ARG 1
    PLA 
    TAY         ; ARG 0
    LDA #$00
    STA $1003
    STX $1004
    STY $1005
DIV_0:
    DEX
    DEY
    CPX #$00
    BNE DIV_0
    LDX $1004
    INC $1003
    CPY #$00
    BNE DIV_0
    CPY $1005
    BEQ DIV_0
    BCC DIV_0
    LDY $1003
    JMP ENDOP
MAIN:           ; ENTRY POINT      
    LDA #02     ; ARG 0
    PHA
    LDA #04     ; ARG 1
    PHA
    LDA #01     ; OP_TYPE = ADD
    PHA   
    JSR CALC    ; X = 2 + 4
    LDA #02
    PHA
    LDA #03     ; OP_TYPE = MULTIPLICATION
    PHA
    JSR CALC    ; X = X * 2
    PLA
    BRK