diff --git a/gamepad.cpp b/gamepad.cpp
index e91b84b..7ea6e73 100644
--- a/gamepad.cpp
+++ b/gamepad.cpp
@@ -1,5 +1,6 @@
 #include "gamepad.h"
 #include <SDL2/SDL.h>
+#include "z80.h"
 #include "zx_ula.h"
 #include <vector>
 
@@ -20,6 +21,30 @@ namespace gamepad
     };
 
     std::vector<gamepad_t> gamepads;
+    bool kempston_active = false;
+
+    int kempston_port_in(int port)
+    {
+        for (auto gamepad: gamepads)
+        {
+            if (gamepad.type == GAMEPAD_TYPE_KEMPSTON)
+            {
+                uint8_t result = 0;
+                if (gamepad.buttons[GAMEPAD_BUTTON_FIRE]) result |= 0x10;
+                if (gamepad.buttons[GAMEPAD_BUTTON_UP]) result |= 0x08;
+                if (gamepad.buttons[GAMEPAD_BUTTON_DOWN]) result |= 0x04;
+                if (gamepad.buttons[GAMEPAD_BUTTON_LEFT]) result |= 0x02;
+                if (gamepad.buttons[GAMEPAD_BUTTON_RIGHT]) result |= 0x01;
+                return result;
+            }
+        }
+        return 0;
+    }
+
+    void init()
+    {
+        z80::connect_port(0x1f, 0x001f, kempston_port_in, nullptr);
+    }
 
     uint8_t getNextType()
     {
@@ -55,25 +80,17 @@ namespace gamepad
         return gamepads.size()-1;
     }
 
-    void modify(int32_t index, uint8_t type)
-    {
-        for (auto& gamepad : gamepads)
-        {
-            if (gamepad.index == index)
-            {
-                gamepad.type = type;
-                return;
-            }
-        }
-    }
-
     void remove(int32_t index)
     {
         SDL_GameController *game_controller = SDL_GameControllerFromInstanceID(index);
         for (int i=0; i<gamepads.size(); ++i)
         {
-            if (gamepads[i].gamepad == game_controller) gamepads.erase(gamepads.begin()+i);
+            if (gamepads[i].gamepad == game_controller) {
+                gamepads.erase(gamepads.begin()+i);
+                break;
+            }
         }
+
         SDL_GameControllerClose(game_controller);
     }
 
diff --git a/gamepad.h b/gamepad.h
index 3fd8bf6..957e08b 100644
--- a/gamepad.h
+++ b/gamepad.h
@@ -10,8 +10,8 @@
 
 namespace gamepad
 {
+    void init();
     uint8_t add(int32_t index, uint8_t type);
-    void modify(int32_t index, uint8_t type);
     void remove(int32_t index);
     void buttonDown(int32_t index, uint8_t button);
     void buttonUp(int32_t index, uint8_t button);
diff --git a/main.cpp b/main.cpp
index eea8ca3..2e1fc64 100644
--- a/main.cpp
+++ b/main.cpp
@@ -169,7 +169,7 @@ int main(int argc, char *argv[])
 
     //uint32_t update_freq = 
     zx_system::init(ZX_48K);
-
+    gamepad::init();
     zx_tape::load("ROBOCOP1.TAP");
 
     //if (argc==3) { z80debug::loadngo(argv[1], argv[2]); }