3 files changed, 52 insertions, 32 deletions

diff --git a/shared-module/gamepad/GamePad.c b/shared-module/gamepad/GamePad.c
index ed574db53..4fea7fe4b 100644
--- a/shared-module/gamepad/GamePad.c
+++ b/shared-module/gamepad/GamePad.c
@@ -56,7 +56,6 @@ void gamepad_init_pins(size_t n_pins, const mp_obj_t* pins) {
         }
         gamepad_singleton->pins[i] = pin;
     }
-    gamepad_singleton->last = 0;
     gamepad_singleton->kind = GAMEPAD_KIND_PINS;
 }
 
@@ -74,6 +73,5 @@ void gamepad_init_shift(digitalio_digitalinout_obj_t *data_pin,
     common_hal_digitalio_digitalinout_switch_to_output(clock_pin, 1, DRIVE_MODE_PUSH_PULL);
     gamepad_singleton->pins[2] = latch_pin;
 
-    gamepad_singleton->last = 0;
     gamepad_singleton->kind = GAMEPAD_KIND_PINS;
 }
diff --git a/shared-module/gamepad/GamePad.h b/shared-module/gamepad/GamePad.h
index 9bc5a711c..8ba597a37 100644
--- a/shared-module/gamepad/GamePad.h
+++ b/shared-module/gamepad/GamePad.h
@@ -34,7 +34,6 @@
 typedef struct {
     mp_obj_base_t base;
     digitalio_digitalinout_obj_t* pins[8];
-    volatile uint8_t last;
     volatile uint8_t pressed;
     uint8_t pulls;
     uint8_t kind;
diff --git a/shared-module/gamepad/__init__.c b/shared-module/gamepad/__init__.c
index c79ee30ba..6f9b80bec 100644
--- a/shared-module/gamepad/__init__.c
+++ b/shared-module/gamepad/__init__.c
@@ -33,46 +33,69 @@
 #include "shared-bindings/digitalio/DigitalInOut.h"
 
 
+void pressed_pins(gamepad_obj_t *self) {
+    static uint8_t last = 0;
+    uint8_t current = 0;
+    uint8_t bit = 1;
+    for (int i = 0; i < 8; ++i) {
+        digitalio_digitalinout_obj_t* pin = self->pins[i];
+        if (!pin) {
+            break;
+        }
+        if (common_hal_digitalio_digitalinout_get_value(pin)) {
+            current |= bit;
+        }
+        bit <<= 1;
+    }
+    current ^= self->pulls;
+    self->pressed |= last & current;
+    last = current;
+}
+
+
+void pressed_shift(gamepad_obj_t *self) {
+    static volatile uint8_t i = 8;
+    static volatile uint8_t clock = 0;
+    digitalio_digitalinout_obj_t* data_pin = self->pins[0];
+    digitalio_digitalinout_obj_t* clock_pin = self->pins[1];
+    digitalio_digitalinout_obj_t* latch_pin = self->pins[2];
+
+    if (clock == 0) {
+        common_hal_digitalio_digitalinout_set_value(clock_pin, 1);
+        clock = 1;
+        return;
+    }
+
+    if (i == 8) {
+        common_hal_digitalio_digitalinout_set_value(latch_pin, 0);
+        i = 9;
+    } else if (i == 9) {
+        common_hal_digitalio_digitalinout_set_value(latch_pin, 1);
+        i = 0;
+    } else {
+        if (common_hal_digitalio_digitalinout_get_value(data_pin)) {
+            self->pressed |= (1 << i);
+        }
+        i += 1;
+    }
+    common_hal_digitalio_digitalinout_set_value(clock_pin, 0);
+    clock = 0;
+}
+
+
 void gamepad_tick(void) {
     gamepad_obj_t* gamepad_singleton = MP_STATE_VM(gamepad_singleton);
     if (!gamepad_singleton) {
         return;
     }
-    uint8_t gamepad_current = 0;
-    uint8_t bit = 1;
     switch (gamepad_singleton->kind) {
     case GAMEPAD_KIND_PINS:
-        for (int i = 0; i < 8; ++i) {
-            digitalio_digitalinout_obj_t* pin = gamepad_singleton->pins[i];
-            if (!pin) {
-                break;
-            }
-            if (common_hal_digitalio_digitalinout_get_value(pin)) {
-                gamepad_current |= bit;
-            }
-            bit <<= 1;
-        }
-        gamepad_current ^= gamepad_singleton->pulls;
+        pressed_pins(gamepad_singleton);
         break;
     case GAMEPAD_KIND_SHIFT:
-        bit = 1;
-        digitalio_digitalinout_obj_t* data_pin = gamepad_singleton->pins[0];
-        digitalio_digitalinout_obj_t* clock_pin = gamepad_singleton->pins[1];
-        digitalio_digitalinout_obj_t* latch_pin = gamepad_singleton->pins[2];
-        common_hal_digitalio_digitalinout_set_value(latch_pin, 0);
-        for (int i = 0; i < 8; ++i) {
-            common_hal_digitalio_digitalinout_set_value(clock_pin, 1);
-            if (common_hal_digitalio_digitalinout_get_value(data_pin)) {
-                gamepad_current |= bit;
-            }
-            bit <<= 1;
-            common_hal_digitalio_digitalinout_set_value(clock_pin, 0);
-        }
-        common_hal_digitalio_digitalinout_set_value(latch_pin, 1);
+        pressed_shift(gamepad_singleton);
         break;
     }
-    gamepad_singleton->pressed |= gamepad_singleton->last & gamepad_current;
-    gamepad_singleton->last = gamepad_current;
 }
 
 void gamepad_reset(void) {