;;;
;;; cpu6502.lisp
;;;
;;; 6502 CPU core (sans decimal mode) for NES emulation
;;;

(in-package :neth)

;;;; CPU variables

(declaim (type (simple-array (unsigned-byte 8) (*)) *cpu-ram*))
(defvar *cpu-ram* (make-array #x800 :element-type '(unsigned-byte 8)
			      :initial-element 0)
  "Storage for the #x0000-#x07ff RAM area")

(declaim (type (simple-array (array (unsigned-byte 8) (*)) (4)) *prg-banks*))
(defvar *prg-banks* (make-array 4) "The PRG-ROM as seen by the CPU")

(declaim (type (simple-array (function) (8)) *cpu-write-handlers* *cpu-read-handlers*))
(defvar *cpu-write-handlers* (make-array 8) "Write handlers for CPU")
(defvar *cpu-read-handlers* (make-array 8) "Read handlers for CPU")
(defvar *cpu* nil "The 6502 CPU")


;;;; Memory interface

(defun cpu-read (address)
  (funcall (aref *cpu-read-handlers* (ldb (byte 3 13) address)) address))

(defun cpu-write (address data)
  (funcall (aref *cpu-write-handlers* (ldb (byte 3 13) address)) address data))


;;;; The CPU structure

(defstruct cpu6502
  (reg-a 0 :type (unsigned-byte 8))
  (reg-x 0 :type (unsigned-byte 8))
  (reg-y 0 :type (unsigned-byte 8))
  (reg-s 0 :type (unsigned-byte 8))
  (reg-p 0 :type (unsigned-byte 8))
  (reg-pc 0 :type (unsigned-byte 16))
  scratch
  cycles-left)


;;;; Disassembly interface

(defun dis6502-curr-instr (cpu)
  (dis6502-one-instr (aref *prg-banks* (ldb (byte 2 13) (cpu6502-reg-pc cpu)))
		     (logand #x1fff (cpu6502-reg-pc cpu)) (cpu6502-reg-pc cpu)))


;;;; Interrupts

(defun cpu6502-reset (cpu)
  (setf (cpu6502-reg-a cpu) 0)
  (setf (cpu6502-reg-x cpu) 0)
  (setf (cpu6502-reg-y cpu) 0)
  (setf (cpu6502-reg-s cpu) 0)
  (setf (cpu6502-reg-pc cpu)
	(dpb (cpu-read #xfffd) (byte 8 8) (cpu-read #xfffc)))
  (setf (cpu6502-cycles-left cpu) 0)
  (dis6502-curr-instr cpu))


;;;; Instruction fetch

(defun cpu6502-opfetch (cpu)
  (declare (type cpu6502 cpu))
;  (declare (optimize (speed 3) (safety 0)))
  (let ((addr (cpu6502-reg-pc cpu)))
    (setf (cpu6502-reg-pc cpu) (logand #xffff (1+ (cpu6502-reg-pc cpu))))
    (cpu-read addr)))

(defun cpu6502-opfetch-16 (cpu)
  (let ((lo (cpu6502-opfetch cpu))
	(hi (cpu6502-opfetch cpu)))
    (dpb hi (byte 8 8) lo)))


;;;; Flag manipulation

(defun cpu6502-flag-nz (cpu data)
  (let ((flags (cpu6502-reg-p cpu)))
    (setf flags (logior (logand #x7d flags)
			(if (zerop data) 2 0)
			(logand #x80 data)))
    (setf (cpu6502-reg-p cpu) flags)))

(defun cpu6502-flag-nzc-add (cpu data)
  (let ((flags (cpu6502-reg-p cpu)))
    (setf flags (logior (logand #x7c flags)
			(if (zerop (logand #xff00 data)) 0 1)
			(if (zerop (logand #xff data)) 2 0)
			(logand #x80 data)))
    (setf (cpu6502-reg-p cpu) flags)))

(defun cpu6502-flag-nzc-sub (cpu data)
  (let ((flags (cpu6502-reg-p cpu)))
    (setf flags (logior (logand #x7c flags)
			(if (zerop (logand #xff00 data)) 1 0)
			(if (zerop (logand #xff data)) 2 0)
			(logand #x80 data)))
    (setf (cpu6502-reg-p cpu) flags)))


;;;; Stack access

(defun cpu6502-push-8 (cpu data)
  (setf (aref *cpu-ram* (logior #x100 (cpu6502-reg-s cpu))) data)
  (setf (cpu6502-reg-s cpu) (logand #xff (1- (cpu6502-reg-s cpu)))))

(defun cpu6502-pop-8 (cpu)
  (setf (cpu6502-reg-s cpu) (logand #xff (1+ (cpu6502-reg-s cpu))))
  (aref *cpu-ram* (logior #x100 (cpu6502-reg-s cpu))))

(defun cpu6502-push-16 (cpu data)
  (cpu6502-push-8 cpu (ldb (byte 8 8) data))
  (cpu6502-push-8 cpu (logand #xff data)))

(defun cpu6502-pop-16 (cpu)
  (let* ((data-lo (cpu6502-pop-8 cpu))
	 (data-hi (cpu6502-pop-8 cpu)))
    (dpb data-hi (byte 8 8) data-lo)))


;;;; Addressing modes

(defun cpu6502-imm-read (cpu continuation)
  (declare (type function continuation))
  (funcall continuation cpu (cpu6502-opfetch cpu)))

(defun cpu6502-acc-rmw (cpu continuation)
  (declare (type function continuation))
  (declare (type cpu6502 cpu))
;  (declare (optimize (speed 3) (safety 0)))
  (funcall continuation cpu (cpu6502-reg-a cpu)
	     (lambda (cpu data) (setf (cpu6502-reg-a cpu) data))))


(defmacro cpu6502-defread (func addrfunc readfunc)
  `(defun ,func (cpu continuation)
    (declare (function continuation))
     (funcall continuation cpu (,readfunc cpu (,addrfunc cpu)))))

(defmacro cpu6502-defwrite (func addrfunc writefunc)
  `(defun ,func (cpu data)
     (,writefunc cpu (,addrfunc cpu) data)))

(defmacro cpu6502-defrmw (func addrfunc readfunc writefunc)
  `(defun ,func (cpu continuation)
    (declare (function continuation))
     (let ((addr (,addrfunc cpu)))
       (setf (cpu6502-scratch cpu) addr)
       (funcall continuation cpu (,readfunc cpu addr)
		(lambda (cpu data)
		  (,writefunc cpu (cpu6502-scratch cpu) data))))))


(defmacro cpu6502-zpg-addrfunc (cpu)
  `(cpu6502-opfetch ,cpu))

(defmacro cpu6502-zpg-readfunc (cpu addr)
  (declare (ignore cpu))
  `(aref *cpu-ram* ,addr))

(defmacro cpu6502-zpg-writefunc (cpu addr data)
  (declare (ignore cpu))
  `(setf (aref *cpu-ram* ,addr) ,data))

(cpu6502-defread cpu6502-zpg-read cpu6502-zpg-addrfunc cpu6502-zpg-readfunc)
(cpu6502-defwrite cpu6502-zpg-write cpu6502-zpg-addrfunc cpu6502-zpg-writefunc)
(cpu6502-defrmw cpu6502-zpg-rmw cpu6502-zpg-addrfunc cpu6502-zpg-readfunc cpu6502-zpg-writefunc)


(defmacro cpu6502-zpgx-addrfunc (cpu)
  `(logand (+ (cpu6502-opfetch ,cpu)
	      (cpu6502-reg-x ,cpu)) #xff))

(cpu6502-defread cpu6502-zpgx-read cpu6502-zpgx-addrfunc cpu6502-zpg-readfunc)
(cpu6502-defwrite cpu6502-zpgx-write cpu6502-zpgx-addrfunc cpu6502-zpg-writefunc)
(cpu6502-defrmw cpu6502-zpgx-rmw cpu6502-zpgx-addrfunc cpu6502-zpg-readfunc cpu6502-zpg-writefunc)


(defmacro cpu6502-zpgy-addrfunc (cpu)
  `(logand (+ (cpu6502-opfetch ,cpu)
	      (cpu6502-reg-y ,cpu)) #xff))

(cpu6502-defread cpu6502-zpgy-read cpu6502-zpgy-addrfunc cpu6502-zpg-readfunc)
(cpu6502-defwrite cpu6502-zpgy-write cpu6502-zpgy-addrfunc cpu6502-zpg-writefunc)


(defmacro cpu6502-abs-addrfunc (cpu)
  `(cpu6502-opfetch-16 ,cpu))

(defmacro cpu6502-abs-readfunc (cpu addr)
  (declare (ignore cpu))
  `(cpu-read ,addr))

(defmacro cpu6502-abs-writefunc (cpu addr data)
  (declare (ignore cpu))
  `(cpu-write ,addr ,data))

(cpu6502-defread cpu6502-abs-read cpu6502-abs-addrfunc cpu6502-abs-readfunc)
(cpu6502-defwrite cpu6502-abs-write cpu6502-abs-addrfunc cpu6502-abs-writefunc)
(cpu6502-defrmw cpu6502-abs-rmw cpu6502-abs-addrfunc cpu6502-abs-readfunc cpu6502-abs-writefunc)


(defmacro cpu6502-absx-addrfunc (cpu)
  `(let ((addr (+ (cpu6502-opfetch ,cpu) (cpu6502-reg-x ,cpu))))
     (when (> #xff addr)
       ;; FIXME: Single-cycle penalty
       )
     (+ addr (dpb (cpu6502-opfetch ,cpu) (byte 8 8) 0))))

(cpu6502-defread cpu6502-absx-read cpu6502-absx-addrfunc cpu6502-abs-readfunc)
(cpu6502-defwrite cpu6502-absx-write cpu6502-absx-addrfunc cpu6502-abs-writefunc)
(cpu6502-defrmw cpu6502-absx-rmw cpu6502-absx-addrfunc cpu6502-abs-readfunc cpu6502-abs-writefunc)


(defmacro cpu6502-absy-addrfunc (cpu)
  `(let ((addr (+ (cpu6502-opfetch ,cpu) (cpu6502-reg-y ,cpu))))
     (when (> #xff addr)
       ;; FIXME: Single-cycle penalty
       )
     (+ addr (dpb (cpu6502-opfetch ,cpu) (byte 8 8) 0))))

(cpu6502-defread cpu6502-absy-read cpu6502-absy-addrfunc cpu6502-abs-readfunc)
(cpu6502-defwrite cpu6502-absy-write cpu6502-absy-addrfunc cpu6502-abs-writefunc)


(defmacro cpu6502-indx-addrfunc (cpu)
  `(let* ((addr1 (logand #xff (+ (cpu6502-opfetch ,cpu)
				 (cpu6502-reg-x ,cpu))))
	  (addr2 (aref *cpu-ram* addr1))
	  (addr3 (logand #xff (1+ addr1))))
     (dpb (aref *cpu-ram* addr3) (byte 8 8) addr2)))

(cpu6502-defread cpu6502-indx-read cpu6502-indx-addrfunc cpu6502-abs-readfunc)
(cpu6502-defwrite cpu6502-indx-write cpu6502-indx-addrfunc cpu6502-abs-writefunc)


(defmacro cpu6502-indy-addrfunc (cpu)
  `(let* ((addr1 (cpu6502-opfetch ,cpu))
	 (addr2 (+ (aref *cpu-ram* addr1) (cpu6502-reg-y ,cpu))))
    (incf addr1)
    (when (> #xff addr2)
      ;; FIXME: Single-cycle penalty
      )
    (+ addr2 (dpb (aref *cpu-ram* addr1) (byte 8 8) 0))))

(cpu6502-defread cpu6502-indy-read cpu6502-indy-addrfunc cpu6502-abs-readfunc)
(cpu6502-defwrite cpu6502-indy-write cpu6502-indy-addrfunc cpu6502-abs-writefunc)


;;;; Instructions

(defun cpu6502-inc (cpu data continuation)
  (declare (type function continuation))
  (declare (type cpu6502 cpu))
  (declare (type (unsigned-byte 8) data))
;  (declare (optimize (speed 3) (safety 0)))
  (let ((result (logand #xff (1+ data))))
    (cpu6502-flag-nz cpu result)
    (funcall continuation cpu result)))

(defun cpu6502-dec (cpu data continuation)
  (declare (type function continuation))
  (let ((result (logand #xff (1- data))))
    (cpu6502-flag-nz cpu result)
    (funcall continuation cpu result)))

(defun cpu6502-asl (cpu data continuation)
  (declare (type function continuation))
  (let ((result (ash data 1)))
    (cpu6502-flag-nzc-add cpu result)
    (funcall continuation cpu (logand #xff result))))

(defun cpu6502-lsr (cpu data continuation)
  (declare (type function continuation))
  (let ((result (ldb (byte 7 1) data)))
    (cpu6502-flag-nz cpu result)
    (setf (cpu6502-reg-p cpu)
	  (logior (logand (cpu6502-reg-p cpu) #xfe)
		  (logand 1 data)))
    (funcall continuation cpu result)))

(defun cpu6502-rol (cpu data continuation)
  (declare (type function continuation))
  (let ((result (dpb data (byte 8 1) (cpu6502-reg-p cpu))))
    (cpu6502-flag-nzc-add cpu result)
    (funcall continuation cpu (logand #xff result))))

(defun cpu6502-ror (cpu data continuation)
  (declare (type function continuation))
  (let ((result (dpb (cpu6502-reg-p cpu) (byte 1 7) (ldb (byte 7 1) data))))
    (cpu6502-flag-nz cpu result)
    (setf (cpu6502-reg-p cpu)
	  (logior (logand (cpu6502-reg-p cpu) #xfe)
		  (logand 1 data)))
    (funcall continuation cpu result)))

(defun cpu6502-jmp-abs (cpu continuation)
  (declare (ignore continuation))
  (let ((addr (cpu6502-opfetch-16 cpu)))
    (setf (cpu6502-reg-pc cpu) addr)))

(defun cpu6502-jmp-ind (cpu continuation)
  (declare (ignore continuation))
  (let* ((addr (cpu6502-opfetch-16 cpu))
	 (addr2 (cpu-read addr))
	 (addr3 (dpb (cpu-read (1+ addr)) (byte 8 8) addr2)))
    (setf (cpu6502-reg-pc cpu) addr3)))

(defun cpu6502-sei (cpu continuation)
  (declare (ignore continuation))
  (setf (cpu6502-reg-p cpu)
	(logior (cpu6502-reg-p cpu) #x04)))

(defun cpu6502-cli (cpu continuation)
  (declare (ignore continuation))
  (setf (cpu6502-reg-p cpu)
	(logand (cpu6502-reg-p cpu) #xfb)))

(defun cpu6502-clv (cpu continuation)
  (declare (ignore continuation))
  (setf (cpu6502-reg-p cpu)
	(logand (cpu6502-reg-p cpu) #xbf)))

(defun cpu6502-cld (cpu continuation)
  (declare (ignore continuation))
  (setf (cpu6502-reg-p cpu)
	(logand (cpu6502-reg-p cpu) #xf7)))

(defun cpu6502-sed (cpu continuation)
  (declare (ignore continuation))
  (setf (cpu6502-reg-p cpu)
	(logior (cpu6502-reg-p cpu) #x08)))

(defun cpu6502-clc (cpu continuation)
  (declare (ignore continuation))
  (setf (cpu6502-reg-p cpu)
	(logand (cpu6502-reg-p cpu) #xfe)))

(defun cpu6502-sec (cpu continuation)
  (declare (ignore continuation))
  (setf (cpu6502-reg-p cpu)
	(logior (cpu6502-reg-p cpu) #x01)))

(defun cpu6502-lda (cpu data)
  (declare (type cpu6502 cpu))
  (declare (type (unsigned-byte 8) data))
;  (declare (optimize (speed 3) (safety 0)))
  (setf (cpu6502-reg-a cpu) data)
  (cpu6502-flag-nz cpu data))

(defun cpu6502-ldy (cpu data)
  (setf (cpu6502-reg-y cpu) data)
  (cpu6502-flag-nz cpu data))

(defun cpu6502-sta (cpu continuation)
  (declare (type function continuation))
  (funcall continuation cpu (cpu6502-reg-a cpu)))

(defun cpu6502-stx (cpu continuation)
  (declare (type function continuation))
  (funcall continuation cpu (cpu6502-reg-x cpu)))

(defun cpu6502-sty (cpu continuation)
  (declare (type function continuation))
  (funcall continuation cpu (cpu6502-reg-y cpu)))

(defun cpu6502-ldx (cpu data)
  (setf (cpu6502-reg-x cpu) data)
  (cpu6502-flag-nz cpu data))

(defun cpu6502-txs (cpu continuation)
  (declare (ignore continuation))
  (setf (cpu6502-reg-s cpu)
	(cpu6502-reg-x cpu)))

(defun cpu6502-tsx (cpu continuation)
  (declare (ignore continuation))
  (setf (cpu6502-reg-x cpu)
	(cpu6502-reg-s cpu))
  (cpu6502-flag-nz cpu (cpu6502-reg-x cpu)))

(defun cpu6502-tax (cpu continuation)
  (declare (ignore continuation))
  (setf (cpu6502-reg-x cpu)
	(cpu6502-reg-a cpu))
  (cpu6502-flag-nz cpu (cpu6502-reg-a cpu)))

(defun cpu6502-txa (cpu continuation)
  (declare (ignore continuation))
  (setf (cpu6502-reg-a cpu)
	(cpu6502-reg-x cpu))
  (cpu6502-flag-nz cpu (cpu6502-reg-a cpu)))

(defun cpu6502-tay (cpu continuation)
  (declare (ignore continuation))
  (setf (cpu6502-reg-y cpu)
	(cpu6502-reg-a cpu))
  (cpu6502-flag-nz cpu (cpu6502-reg-a cpu)))

(defun cpu6502-tya (cpu continuation)
  (declare (ignore continuation))
  (setf (cpu6502-reg-a cpu)
	(cpu6502-reg-y cpu))
  (cpu6502-flag-nz cpu (cpu6502-reg-a cpu)))

(defun cpu6502-bxx (cpu continuation)
  (declare (type function continuation))
  (let ((offset (u8->s8 (cpu6502-opfetch cpu))))
    (funcall continuation cpu
	     (logand (+ (cpu6502-reg-pc cpu) offset) #xffff))))

(defun cpu6502-bpl (cpu addr)
  (when (zerop (logand (cpu6502-reg-p cpu) #x80))
    (setf (cpu6502-reg-pc cpu) addr)))

(defun cpu6502-bmi (cpu addr)
  (unless (zerop (logand (cpu6502-reg-p cpu) #x80))
    (setf (cpu6502-reg-pc cpu) addr)))

(defun cpu6502-bvc (cpu addr)
  (when (zerop (logand (cpu6502-reg-p cpu) #x40))
    (setf (cpu6502-reg-pc cpu) addr)))

(defun cpu6502-bvs (cpu addr)
  (unless (zerop (logand (cpu6502-reg-p cpu) #x40))
    (setf (cpu6502-reg-pc cpu) addr)))

(defun cpu6502-bcc (cpu addr)
  (when (zerop (logand (cpu6502-reg-p cpu) #x01))
    (setf (cpu6502-reg-pc cpu) addr)))

(defun cpu6502-bcs (cpu addr)
  (unless (zerop (logand (cpu6502-reg-p cpu) #x01))
    (setf (cpu6502-reg-pc cpu) addr)))

(defun cpu6502-bne (cpu addr)
  (when (zerop (logand (cpu6502-reg-p cpu) #x02))
    (setf (cpu6502-reg-pc cpu) addr)))

(defun cpu6502-beq (cpu addr)
  (unless (zerop (logand (cpu6502-reg-p cpu) #x02))
    (setf (cpu6502-reg-pc cpu) addr)))

(defun cpu6502-nop (cpu continuation)
  (declare (ignore cpu continuation)))

(defun cpu6502-pha (cpu continuation)
  (declare (ignore continuation))
  (cpu6502-push-8 cpu (cpu6502-reg-a cpu)))

(defun cpu6502-pla (cpu continuation)
  (declare (ignore continuation))
  (let ((result (cpu6502-pop-8 cpu)))
    (setf (cpu6502-reg-a cpu) result)
    (cpu6502-flag-nz cpu result)))

(defun cpu6502-php (cpu continuation)
  (declare (ignore continuation))
  (cpu6502-push-8 cpu (cpu6502-reg-p cpu)))

(defun cpu6502-plp (cpu continuation)
  (declare (ignore continuation))
  (setf (cpu6502-reg-p cpu) (cpu6502-pop-8 cpu)))

(defun cpu6502-jsr-abs (cpu continuation)
  (declare (ignore continuation))
  (cpu6502-push-16 cpu (+ 1 (cpu6502-reg-pc cpu)))
  (setf (cpu6502-reg-pc cpu) (cpu6502-opfetch-16 cpu)))

(defun cpu6502-rts (cpu continuation)
  (declare (ignore continuation))
  (setf (cpu6502-reg-pc cpu) (logand #xffff (1+ (cpu6502-pop-16 cpu)))))

(defun cpu6502-and (cpu data)
  (setf (cpu6502-reg-a cpu) (logand (cpu6502-reg-a cpu) data))
  (cpu6502-flag-nz cpu (cpu6502-reg-a cpu)))

(defun cpu6502-ora (cpu data)
  (setf (cpu6502-reg-a cpu) (logior (cpu6502-reg-a cpu) data))
  (cpu6502-flag-nz cpu (cpu6502-reg-a cpu)))

(defun cpu6502-eor (cpu data)
  (setf (cpu6502-reg-a cpu) (logxor (cpu6502-reg-a cpu) data))
  (cpu6502-flag-nz cpu (cpu6502-reg-a cpu)))

(defun cpu6502-bit (cpu data)
  (setf (cpu6502-reg-p cpu) (logior (logand #x3d (cpu6502-reg-p cpu))
				    (logand #xc0 data)
				    (if (zerop (logand (cpu6502-reg-a cpu) data)) 2 0))))

(defun cpu6502-cmp (cpu data)
  (cpu6502-flag-nzc-sub cpu (- (cpu6502-reg-a cpu) data)))

(defun cpu6502-cpx (cpu data)
  (cpu6502-flag-nzc-sub cpu (- (cpu6502-reg-x cpu) data)))

(defun cpu6502-cpy (cpu data)
  (cpu6502-flag-nzc-sub cpu (- (cpu6502-reg-y cpu) data)))

(defun cpu6502-adc (cpu data)
  (let ((result (+ (cpu6502-reg-a cpu) data
		   (logand 1 (cpu6502-reg-p cpu)))))
    (setf (cpu6502-reg-p cpu)
	  (logior (logand (cpu6502-reg-p cpu) #xbf)
		  (ash (logand (logxor result data)
			       (logxor (cpu6502-reg-a cpu) result)
			       #x80) -1)))
    (cpu6502-flag-nzc-add cpu result)
    (setf (cpu6502-reg-a cpu) (logand result #xff))))

(defun cpu6502-sbc (cpu data)
  (let ((result (- (cpu6502-reg-a cpu) data
		   (- 1 (logand 1 (cpu6502-reg-p cpu))))))
    (setf (cpu6502-reg-p cpu)
	  (logior (logand (cpu6502-reg-p cpu) #xbf)
		  (ash (logand (logxor (cpu6502-reg-a cpu) data)
			       (logxor (cpu6502-reg-a cpu) result)
			       #x80) -1)))
    (cpu6502-flag-nzc-sub cpu result)
    (setf (cpu6502-reg-a cpu) (logand result #xff))))

(defun cpu6502-iny (cpu continuation)
  (declare (ignore continuation))
  (setf (cpu6502-reg-y cpu) (logand #xff (1+ (cpu6502-reg-y cpu))))
  (cpu6502-flag-nz cpu (cpu6502-reg-y cpu)))

(defun cpu6502-inx (cpu continuation)
  (declare (ignore continuation))
  (setf (cpu6502-reg-x cpu) (logand #xff (1+ (cpu6502-reg-x cpu))))
  (cpu6502-flag-nz cpu (cpu6502-reg-x cpu)))

(defun cpu6502-dey (cpu continuation)
  (declare (ignore continuation))
  (setf (cpu6502-reg-y cpu) (logand #xff (1- (cpu6502-reg-y cpu))))
  (cpu6502-flag-nz cpu (cpu6502-reg-y cpu)))

(defun cpu6502-dex (cpu continuation)
  (declare (ignore continuation))
  (setf (cpu6502-reg-x cpu) (logand #xff (1- (cpu6502-reg-x cpu))))
  (cpu6502-flag-nz cpu (cpu6502-reg-x cpu)))

(defun cpu6502-rti (cpu continuation)
  (declare (ignore continuation))
  (setf (cpu6502-reg-p cpu) (cpu6502-pop-8 cpu))
  (setf (cpu6502-reg-pc cpu) (cpu6502-pop-16 cpu)))

(defun cpu6502-brk (cpu continuation)
  (declare (ignore continuation))
  (cpu6502-push-16 cpu (logand #xffff (1+ (cpu6502-reg-pc cpu))))
  (cpu6502-push-8 cpu (logior (cpu6502-reg-p cpu) #x10))
  (setf (cpu6502-reg-p cpu) (logior #x04 (cpu6502-reg-p cpu)))
  (setf (cpu6502-reg-pc cpu) (let ((addr (cpu-read #xfffe)))
			       (dpb (cpu-read #xffff) (byte 8 8) addr))))


;;;; Interrupts

(defun cpu6502-nmi (cpu)
  (cpu6502-push-16 cpu (cpu6502-reg-pc cpu))
  (cpu6502-push-8 cpu (logand (cpu6502-reg-p cpu) #xef))
  (setf (cpu6502-reg-pc cpu) (let ((addr (cpu-read #xfffa)))
			       (dpb (cpu-read #xfffb) (byte 8 8) addr))))

(defun cpu6502-irq (cpu)
  (cpu6502-push-16 cpu (cpu6502-reg-pc cpu))
  (cpu6502-push-8 cpu (logand (cpu6502-reg-p cpu) #xef))
  (setf (cpu6502-reg-pc cpu) (let ((addr (cpu-read #xfffe)))
			       (dpb (cpu-read #xffff) (byte 8 8) addr))))


;;;; Instruction decoding

(defun cpu6502-unimp (cpu continuation)
  (declare (ignore cpu continuation))
  (error "Invalid opcode hit during instruction emulation."))

(defstruct cpu6502-opcode
  (handler nil :type function)
  continuation
  cycles)

(declaim (type (simple-array cpu6502-opcode (#x100)) *cpu6502-opcodes*))
(defvar *cpu6502-opcodes* (make-array #x100 :element-type 'cpu6502-opcode
				      :initial-element (make-cpu6502-opcode :handler #'cpu6502-unimp :cycles 2))
  "The opcode list for the cpu6502 code")

(defun cpu6502-defop (opcode handler continuation cycles)
  (setf (aref *cpu6502-opcodes* opcode)
	(make-cpu6502-opcode :handler handler
			     :continuation continuation :cycles cycles)))

(cpu6502-defop #x00 #'cpu6502-brk         nil                  2)
(cpu6502-defop #x01 #'cpu6502-indx-read   #'cpu6502-ora        2)
(cpu6502-defop #x05 #'cpu6502-zpg-read    #'cpu6502-ora        2)
(cpu6502-defop #x06 #'cpu6502-zpg-rmw     #'cpu6502-asl        2)
(cpu6502-defop #x08 #'cpu6502-php         nil                  2)
(cpu6502-defop #x09 #'cpu6502-imm-read    #'cpu6502-ora        2)
(cpu6502-defop #x0a #'cpu6502-acc-rmw     #'cpu6502-asl        2)
(cpu6502-defop #x0d #'cpu6502-abs-read    #'cpu6502-ora        2)
(cpu6502-defop #x0e #'cpu6502-abs-rmw     #'cpu6502-asl        2)
(cpu6502-defop #x10 #'cpu6502-bxx         #'cpu6502-bpl        2)
(cpu6502-defop #x11 #'cpu6502-indy-read   #'cpu6502-ora        2)
(cpu6502-defop #x15 #'cpu6502-zpgx-read   #'cpu6502-ora        2)
(cpu6502-defop #x16 #'cpu6502-zpgx-rmw    #'cpu6502-asl        2)
(cpu6502-defop #x18 #'cpu6502-clc         nil                  2)
(cpu6502-defop #x19 #'cpu6502-absy-read   #'cpu6502-ora        2)
(cpu6502-defop #x1d #'cpu6502-absx-read   #'cpu6502-ora        2)
(cpu6502-defop #x1e #'cpu6502-absx-rmw    #'cpu6502-asl        2)
(cpu6502-defop #x20 #'cpu6502-jsr-abs     nil                  2)
(cpu6502-defop #x21 #'cpu6502-indx-read   #'cpu6502-and        2)
(cpu6502-defop #x24 #'cpu6502-zpg-read    #'cpu6502-bit        2)
(cpu6502-defop #x25 #'cpu6502-zpg-read    #'cpu6502-and        2)
(cpu6502-defop #x26 #'cpu6502-zpg-rmw     #'cpu6502-rol        2)
(cpu6502-defop #x28 #'cpu6502-plp         nil                  2)
(cpu6502-defop #x29 #'cpu6502-imm-read    #'cpu6502-and        2)
(cpu6502-defop #x2a #'cpu6502-acc-rmw     #'cpu6502-rol        2)
(cpu6502-defop #x2c #'cpu6502-abs-read    #'cpu6502-bit        2)
(cpu6502-defop #x2d #'cpu6502-abs-read    #'cpu6502-and        2)
(cpu6502-defop #x2e #'cpu6502-abs-rmw     #'cpu6502-rol        2)
(cpu6502-defop #x30 #'cpu6502-bxx         #'cpu6502-bmi        2)
(cpu6502-defop #x31 #'cpu6502-indy-read   #'cpu6502-and        2)
(cpu6502-defop #x35 #'cpu6502-zpgx-read   #'cpu6502-and        2)
(cpu6502-defop #x36 #'cpu6502-zpgx-rmw    #'cpu6502-rol        2)
(cpu6502-defop #x38 #'cpu6502-sec         nil                  2)
(cpu6502-defop #x39 #'cpu6502-absy-read   #'cpu6502-and        2)
(cpu6502-defop #x3d #'cpu6502-absx-read   #'cpu6502-and        2)
(cpu6502-defop #x3e #'cpu6502-absx-rmw    #'cpu6502-rol        2)
(cpu6502-defop #x40 #'cpu6502-rti         nil                  2)
(cpu6502-defop #x41 #'cpu6502-indx-read   #'cpu6502-eor        2)
(cpu6502-defop #x45 #'cpu6502-zpg-read    #'cpu6502-eor        2)
(cpu6502-defop #x46 #'cpu6502-zpg-rmw     #'cpu6502-lsr        2)
(cpu6502-defop #x48 #'cpu6502-pha         nil                  2)
(cpu6502-defop #x49 #'cpu6502-imm-read    #'cpu6502-eor        2)
(cpu6502-defop #x4a #'cpu6502-acc-rmw     #'cpu6502-lsr        2)
(cpu6502-defop #x4c #'cpu6502-jmp-abs     nil                  2)
(cpu6502-defop #x4d #'cpu6502-abs-read    #'cpu6502-eor        2)
(cpu6502-defop #x4e #'cpu6502-abs-rmw     #'cpu6502-lsr        2)
(cpu6502-defop #x50 #'cpu6502-bxx         #'cpu6502-bvc        2)
(cpu6502-defop #x51 #'cpu6502-indy-read   #'cpu6502-eor        2)
(cpu6502-defop #x55 #'cpu6502-zpgx-read   #'cpu6502-eor        2)
(cpu6502-defop #x56 #'cpu6502-zpgx-rmw    #'cpu6502-lsr        2)
(cpu6502-defop #x58 #'cpu6502-cli         nil                  2)
(cpu6502-defop #x59 #'cpu6502-absy-read   #'cpu6502-eor        2)
(cpu6502-defop #x5d #'cpu6502-absx-read   #'cpu6502-eor        2)
(cpu6502-defop #x5e #'cpu6502-absx-rmw    #'cpu6502-lsr        2)
(cpu6502-defop #x60 #'cpu6502-rts         nil                  2)
(cpu6502-defop #x61 #'cpu6502-indx-read   #'cpu6502-adc        2)
(cpu6502-defop #x65 #'cpu6502-zpg-read    #'cpu6502-adc        2)
(cpu6502-defop #x66 #'cpu6502-zpg-rmw     #'cpu6502-ror        2)
(cpu6502-defop #x68 #'cpu6502-pla         nil                  2)
(cpu6502-defop #x69 #'cpu6502-imm-read    #'cpu6502-adc        2)
(cpu6502-defop #x6a #'cpu6502-acc-rmw     #'cpu6502-ror        2)
(cpu6502-defop #x6c #'cpu6502-jmp-ind     nil                  2)
(cpu6502-defop #x6d #'cpu6502-abs-read    #'cpu6502-adc        2)
(cpu6502-defop #x6e #'cpu6502-abs-rmw     #'cpu6502-ror        2)
(cpu6502-defop #x70 #'cpu6502-bxx         #'cpu6502-bvs        2)
(cpu6502-defop #x71 #'cpu6502-indy-read   #'cpu6502-adc        2)
(cpu6502-defop #x75 #'cpu6502-zpgx-read   #'cpu6502-adc        2)
(cpu6502-defop #x76 #'cpu6502-zpgx-rmw    #'cpu6502-ror        2)
(cpu6502-defop #x78 #'cpu6502-sei         nil                  2)
(cpu6502-defop #x79 #'cpu6502-absy-read   #'cpu6502-adc        2)
(cpu6502-defop #x7d #'cpu6502-absx-read   #'cpu6502-adc        2)
(cpu6502-defop #x7e #'cpu6502-absx-rmw    #'cpu6502-ror        2)
(cpu6502-defop #x81 #'cpu6502-sta         #'cpu6502-indx-write 2)
(cpu6502-defop #x84 #'cpu6502-sty         #'cpu6502-zpg-write  2)
(cpu6502-defop #x85 #'cpu6502-sta         #'cpu6502-zpg-write  2)
(cpu6502-defop #x86 #'cpu6502-stx         #'cpu6502-zpg-write  2)
(cpu6502-defop #x88 #'cpu6502-dey         nil                  2)
(cpu6502-defop #x8a #'cpu6502-txa         nil                  2)
(cpu6502-defop #x8c #'cpu6502-sty         #'cpu6502-abs-write  2)
(cpu6502-defop #x8d #'cpu6502-sta         #'cpu6502-abs-write  2)
(cpu6502-defop #x8e #'cpu6502-stx         #'cpu6502-abs-write  2)
(cpu6502-defop #x90 #'cpu6502-bxx         #'cpu6502-bcc        2)
(cpu6502-defop #x91 #'cpu6502-sta         #'cpu6502-indy-write 2)
(cpu6502-defop #x94 #'cpu6502-sty         #'cpu6502-zpgx-write 2)
(cpu6502-defop #x95 #'cpu6502-sta         #'cpu6502-zpgx-write 2)
(cpu6502-defop #x96 #'cpu6502-stx         #'cpu6502-zpgy-write 2)
(cpu6502-defop #x98 #'cpu6502-tya         nil                  2)
(cpu6502-defop #x99 #'cpu6502-sta         #'cpu6502-absy-write 2)
(cpu6502-defop #x9a #'cpu6502-txs         nil                  2)
(cpu6502-defop #x9d #'cpu6502-sta         #'cpu6502-absx-write 2)
(cpu6502-defop #xa0 #'cpu6502-imm-read    #'cpu6502-ldy        2)
(cpu6502-defop #xa1 #'cpu6502-indx-read   #'cpu6502-lda        2)
(cpu6502-defop #xa2 #'cpu6502-imm-read    #'cpu6502-ldx        2)
(cpu6502-defop #xa4 #'cpu6502-zpg-read    #'cpu6502-ldy        2)
(cpu6502-defop #xa5 #'cpu6502-zpg-read    #'cpu6502-lda        2)
(cpu6502-defop #xa6 #'cpu6502-zpg-read    #'cpu6502-ldx        2)
(cpu6502-defop #xa8 #'cpu6502-tay         nil                  2)
(cpu6502-defop #xa9 #'cpu6502-imm-read    #'cpu6502-lda        2)
(cpu6502-defop #xaa #'cpu6502-tax         nil                  2)
(cpu6502-defop #xac #'cpu6502-abs-read    #'cpu6502-ldy        2)
(cpu6502-defop #xad #'cpu6502-abs-read    #'cpu6502-lda        2)
(cpu6502-defop #xae #'cpu6502-abs-read    #'cpu6502-ldx        2)
(cpu6502-defop #xb0 #'cpu6502-bxx         #'cpu6502-bcs        2)
(cpu6502-defop #xb1 #'cpu6502-indy-read   #'cpu6502-lda        2)
(cpu6502-defop #xb4 #'cpu6502-zpgx-read   #'cpu6502-ldy        2)
(cpu6502-defop #xb5 #'cpu6502-zpgx-read   #'cpu6502-lda        2)
(cpu6502-defop #xb6 #'cpu6502-zpgy-read   #'cpu6502-ldx        2)
(cpu6502-defop #xb8 #'cpu6502-clv         nil                  2)
(cpu6502-defop #xb9 #'cpu6502-absy-read   #'cpu6502-lda        2)
(cpu6502-defop #xba #'cpu6502-tsx         nil                  2)
(cpu6502-defop #xbc #'cpu6502-absx-read   #'cpu6502-ldy        2)
(cpu6502-defop #xbd #'cpu6502-absx-read   #'cpu6502-lda        2)
(cpu6502-defop #xbe #'cpu6502-absy-read   #'cpu6502-ldx        2)
(cpu6502-defop #xc0 #'cpu6502-imm-read    #'cpu6502-cpy        2)
(cpu6502-defop #xc1 #'cpu6502-indx-read   #'cpu6502-cmp        2)
(cpu6502-defop #xc4 #'cpu6502-zpg-read    #'cpu6502-cpy        2)
(cpu6502-defop #xc5 #'cpu6502-zpg-read    #'cpu6502-cmp        2)
(cpu6502-defop #xc6 #'cpu6502-zpg-rmw     #'cpu6502-dec        2)
(cpu6502-defop #xc8 #'cpu6502-iny         nil                  2)
(cpu6502-defop #xc9 #'cpu6502-imm-read    #'cpu6502-cmp        2)
(cpu6502-defop #xca #'cpu6502-dex         nil                  2)
(cpu6502-defop #xcc #'cpu6502-abs-read    #'cpu6502-cpy        2)
(cpu6502-defop #xcd #'cpu6502-abs-read    #'cpu6502-cmp        2)
(cpu6502-defop #xce #'cpu6502-abs-rmw     #'cpu6502-dec        2)
(cpu6502-defop #xd0 #'cpu6502-bxx         #'cpu6502-bne        2)
(cpu6502-defop #xd1 #'cpu6502-indy-read   #'cpu6502-cmp        2)
(cpu6502-defop #xd5 #'cpu6502-zpgx-read   #'cpu6502-cmp        2)
(cpu6502-defop #xd6 #'cpu6502-zpgx-rmw    #'cpu6502-dec        2)
(cpu6502-defop #xd8 #'cpu6502-cld         nil                  2)
(cpu6502-defop #xd9 #'cpu6502-absy-read   #'cpu6502-cmp        2)
(cpu6502-defop #xdd #'cpu6502-absx-read   #'cpu6502-cmp        2)
(cpu6502-defop #xde #'cpu6502-absx-rmw    #'cpu6502-dec        2)
(cpu6502-defop #xe0 #'cpu6502-imm-read    #'cpu6502-cpx        2)
(cpu6502-defop #xe1 #'cpu6502-indx-read   #'cpu6502-sbc        2)
(cpu6502-defop #xe4 #'cpu6502-zpg-read    #'cpu6502-cpx        2)
(cpu6502-defop #xe5 #'cpu6502-zpg-read    #'cpu6502-sbc        2)
(cpu6502-defop #xe6 #'cpu6502-zpg-rmw     #'cpu6502-inc        2)
(cpu6502-defop #xe8 #'cpu6502-inx         nil                  2)
(cpu6502-defop #xe9 #'cpu6502-imm-read    #'cpu6502-sbc        2)
(cpu6502-defop #xea #'cpu6502-nop         nil                  2)
(cpu6502-defop #xec #'cpu6502-abs-read    #'cpu6502-cpx        2)
(cpu6502-defop #xed #'cpu6502-abs-read    #'cpu6502-sbc        2)
(cpu6502-defop #xee #'cpu6502-abs-rmw     #'cpu6502-inc        2)
(cpu6502-defop #xf0 #'cpu6502-bxx         #'cpu6502-beq        2)
(cpu6502-defop #xf1 #'cpu6502-indy-read   #'cpu6502-sbc        2)
(cpu6502-defop #xf5 #'cpu6502-zpgx-read   #'cpu6502-sbc        2)
(cpu6502-defop #xf6 #'cpu6502-zpgx-rmw    #'cpu6502-inc        2)
(cpu6502-defop #xf8 #'cpu6502-sed         nil                  2)
(cpu6502-defop #xf9 #'cpu6502-absy-read   #'cpu6502-sbc        2)
(cpu6502-defop #xfd #'cpu6502-absx-read   #'cpu6502-sbc        2)
(cpu6502-defop #xfe #'cpu6502-absx-rmw    #'cpu6502-inc        2)


;;;; Instruction dispatch

(defun cpu6502-step (cpu)
  (let* ((opcode (cpu6502-opfetch cpu))
	 (opdef (aref *cpu6502-opcodes* opcode)))
    (if (not (eq #'cpu6502-unimp (cpu6502-opcode-handler opdef)))
	(funcall (cpu6502-opcode-handler opdef) cpu (cpu6502-opcode-continuation opdef))
      (format t "Unrecognized opcode #x~X~%" opcode)))
  (dis6502-curr-instr cpu))

(defun cpu6502-run (cpu cycles)
  (incf (cpu6502-cycles-left cpu) cycles)
  (do () ((<= (cpu6502-cycles-left cpu) 0))
    (let* ((opcode (cpu6502-opfetch cpu))
	   (opdef (aref *cpu6502-opcodes* opcode)))
      (if (not (eq #'cpu6502-unimp (cpu6502-opcode-handler opdef)))
	  (progn
	    (decf (cpu6502-cycles-left cpu) (cpu6502-opcode-cycles opdef))
	    (funcall (cpu6502-opcode-handler opdef) cpu (cpu6502-opcode-continuation opdef)))
	(progn
	  (format t "Unrecognized opcode #x~X at PC #x~X~%" opcode (1- (cpu6502-reg-pc cpu)))
	  (dis6502-n-instrs (aref *prg-banks* (ldb (byte 2 13) (cpu6502-reg-pc cpu))) (logand #x1fff (- (cpu6502-reg-pc cpu) 10)) (- (cpu6502-reg-pc cpu) 10) 20)
	  (return-from cpu6502-run))))))


;;; EOF
