1 files changed, 72 insertions, 0 deletions
diff --git a/include/joypad.s b/include/joypad.s
new file mode 100644
index 0000000..141877f
--- /dev/null
+++ b/include/joypad.s
@@ -0,0 +1,72 @@
+.scope Joypad
+    ;; Button masks.
+    BUTTON_A      = 1 << 7
+    BUTTON_B      = 1 << 6
+    BUTTON_SELECT = 1 << 5
+    BUTTON_START  = 1 << 4
+    BUTTON_UP     = 1 << 3
+    BUTTON_DOWN   = 1 << 2
+    BUTTON_LEFT   = 1 << 1
+    BUTTON_RIGHT  = 1 << 0
+
+    ;; Port addresses for controllers.
+    JOYPAD1 = $4016
+    JOYPAD2 = $4017
+
+    ;; After running a `read_*` function these two variables will contain the
+    ;; given result.
+    zp_buttons1 = $22
+    zp_buttons2 = $23
+
+    ;;;
+    ;; Safely read a controller via a re-read algorithm the joypad as indexed by
+    ;; the X register (0 for controller 1; 1 for controller 2).
+    .proc read_x
+        jsr Joypad::unsafe_read_x
+
+        ;; The main idea around a re-read algorithm is that you read the
+        ;; controller "unsafely" once, then you do it again and compare both
+        ;; reads. If they were the same then we are on the safe side. Otherwise
+        ;; we would need to loop until we get two identical reads. This sounds
+        ;; bad but in practice it's not so much (and hey, if it worked for Super
+        ;; Mario Bros. 3, it should work for us too :P). Otherwise there is the
+        ;; algorithm via OAM DMA, but it sure is tricky.
+    @reread:
+        lda Joypad::zp_buttons1, x
+        tay
+        jsr Joypad::unsafe_read_x
+        tya
+        cmp Joypad::zp_buttons1, x
+        bne @reread
+
+        rts
+    .endproc
+
+    ;;;
+    ;; Read the joypad as indexed by the X register (0 for controller 1; 1 for
+    ;; controller 2). This method is fast but it might be vulnerable to the DPCM
+    ;; bug (see: https://www.nesdev.org/wiki/Controller_reading_code).
+    .proc unsafe_read_x
+        ;; Start the latch process.
+        lda #$01
+        sta Joypad::JOYPAD1
+        sta Joypad::zp_buttons1, x   ; Bit as a guard for the loop below.
+        lsr
+        sta Joypad::JOYPAD1
+
+        ;; Now the joypad is ready to accept reads.
+    @loop:
+        lda Joypad::JOYPAD1, x
+        and #%00000011              ; Ignore bits other than controller.
+        cmp #$01                    ; Set carry if and only if nonzero.
+        rol Joypad::zp_buttons1, x   ; Carry -> bit 0; bit 7 -> Carry
+        bcc @loop
+        rts
+    .endproc
+.endscope
+
+;; Shortcut for reading the joypad from the first player safely.
+.macro READ_JOYPAD1
+    ldx #$00
+    jsr Joypad::read_x
+.endmacro