From 87a96ab0f81f8df782549beed76567bb50d7e88a Mon Sep 17 00:00:00 2001
From: MickGyver <wmc_mick@yahoo.com>
Date: Tue, 2 Jun 2020 08:45:15 +0300
Subject: [PATCH] Optimisations made to some adapters

- SEGA two player adapter optimisations.
- NES/SNES adapter optimisations.
- Two ports default for Saturn adapter, increased default SELECT_PAUSE.
---
 SNESControllersUSB/SNESControllersUSB.ino     |  77 ++++++-
 SaturnControllerUSB/SaturnControllerUSB.ino   |  10 +-
 SegaControllerUSB/SegaControllerUSB.ino       |   1 -
 SegaTwoControllersUSB/SegaControllers32U4.cpp | 195 +++++++++++++-----
 SegaTwoControllersUSB/SegaControllers32U4.h   |  20 +-
 .../SegaTwoControllersUSB.ino                 |  23 +--
 6 files changed, 229 insertions(+), 97 deletions(-)

diff --git a/SNESControllersUSB/SNESControllersUSB.ino b/SNESControllersUSB/SNESControllersUSB.ino
index fc6e4bb..b975855 100644
--- a/SNESControllersUSB/SNESControllersUSB.ino
+++ b/SNESControllersUSB/SNESControllersUSB.ino
@@ -27,13 +27,13 @@
 // Additionally serial number is used to differentiate arduino projects to have different button maps!
 const char *gp_serial = "NES/SNES to USB";
 
-#define GAMEPAD_COUNT 2     // NOTE: No more than TWO gamepads are possible at the moment due to a USB HID issue.
-#define GAMEPAD_COUNT_MAX 4 // NOTE: For some reason, can't have more than two gamepads without serial breaking. Can someone figure out why?
-                            //       (It has something to do with how Arduino handles HID devices)
-#define BUTTON_READ_DELAY 30 // Delay between button reads in µs
-#define MICROS_LATCH      12 // 12µs according to specs (8 seems to work fine)
-#define MICROS_CLOCK       6 //  6µs according to specs (4 seems to work fine)
-#define MICROS_PAUSE       6 //  6µs according to specs (4 seems to work fine)
+#define GAMEPAD_COUNT 2      // NOTE: No more than TWO gamepads are possible at the moment due to a USB HID issue.
+#define GAMEPAD_COUNT_MAX 4  // NOTE: For some reason, can't have more than two gamepads without serial breaking. Can someone figure out why?
+                             //       (It has something to do with how Arduino handles HID devices)
+#define BUTTON_READ_DELAY 20 // Delay between button reads in µs
+#define MICROS_LATCH      10 // 12µs according to specs (8 seems to work fine)
+#define MICROS_CLOCK       5 //  6µs according to specs (4 seems to work fine)
+#define MICROS_PAUSE       5 //  6µs according to specs (4 seems to work fine)
 
 #define UP    0x01
 #define DOWN  0x02
@@ -55,6 +55,13 @@ const char *gp_serial = "NES/SNES to USB";
 // D1   (GP3: DATA)          A2  (PF5, Gamepad 3, not currently used)
 // D1   (GP4: DATA)          A3  (PF4, Gamepad 4, not currently used)
 
+enum ControllerType {
+  NONE,
+  NES,
+  SNES,
+  NTT
+};
+
 // Set up USB HID gamepads
 Gamepad_ Gamepad[GAMEPAD_COUNT];
 
@@ -62,6 +69,7 @@ Gamepad_ Gamepad[GAMEPAD_COUNT];
 uint32_t buttons[GAMEPAD_COUNT_MAX] = {0,0,0,0};
 uint32_t buttonsPrev[GAMEPAD_COUNT_MAX] = {0,0,0,0};
 uint8_t gpBit[GAMEPAD_COUNT_MAX] = {B10000000,B01000000,B00100000,B00010000};
+ControllerType controllerType[GAMEPAD_COUNT_MAX] = {NONE,NONE};
 uint32_t btnBits[32] = {0x10,0x40,0x400,0x800,UP,DOWN,LEFT,RIGHT,0x20,0x80,0x100,0x200,          // Standard SNES controller
                         0x10000000,0x20000000,0x40000000,0x80000000,0x1000,0x2000,0x4000,0x8000, // NTT Data Keypad (NDK10)
                         0x10000,0x20000,0x40000,0x80000,0x100000,0x200000,0x400000,0x800000,
@@ -81,12 +89,15 @@ void setup()
   // Setup data pins (A0-A3 or PF7-PF4)
   DDRF  &= ~B11110000; // inputs
   PORTF |=  B11110000; // enable internal pull-ups
+
+  delay(500);
+  detectControllerTypes();
 }
 
 void loop() { while(1)
 {
   // See if enough time has passed since last button read
-  if(micros() - microsButtons > BUTTON_READ_DELAY)
+  if((micros() - microsButtons) > BUTTON_READ_DELAY)
   {    
     // Pulse latch
     sendLatch();
@@ -101,14 +112,14 @@ void loop() { while(1)
     // Check gamepad type
     for(gp=0; gp<GAMEPAD_COUNT; gp++) 
     {
-      if((buttons[gp] & 0xF3A0) == 0xF3A0) { // NES
+      if(controllerType[gp] == NES) {    // NES
         bitWrite(buttons[gp], 5, bitRead(buttons[gp], 4));
         bitWrite(buttons[gp], 4, bitRead(buttons[gp], 6));
         buttons[gp] &= 0xC3F;
       }
-      else if(buttons[gp] & NTT_CONTROL_BIT) // SNES NTT Data Keypad
+      else if(controllerType[gp] == NTT) // SNES NTT Data Keypad
         buttons[gp] &= 0x3FFFFFF;
-      else                                   // SNES Gamepad
+      else                               // SNES Gamepad
         buttons[gp] &= 0xFFF; 
     }
 
@@ -129,6 +140,50 @@ void loop() { while(1)
   }
 }}
 
+void detectControllerTypes()
+{
+  uint8_t buttonCountNew = 0;
+
+  // Read the controllers a few times to detect controller type
+  for(uint8_t i=0; i<4; i++) 
+  {
+    // Pulse latch
+    sendLatch();
+
+    // Read all buttons
+    for(uint8_t btn=0; btn<buttonCount; btn++)
+    {
+      for(gp=0; gp<GAMEPAD_COUNT; gp++) 
+        (PINF & gpBit[gp]) ? buttons[gp] &= ~btnBits[btn] : buttons[gp] |= btnBits[btn];
+      sendClock();
+    }
+
+    // Check controller types and set buttonCount to max needed
+    for(gp=0; gp<GAMEPAD_COUNT; gp++) 
+    {
+      if((buttons[gp] & 0xF3A0) == 0xF3A0) {   // NES
+        if(controllerType[gp] != SNES && controllerType[gp] != NTT)
+          controllerType[gp] = NES;
+        if(buttonCountNew < 8)
+          buttonCountNew = 8;
+      }
+      else if(buttons[gp] & NTT_CONTROL_BIT) { // SNES NTT Data Keypad
+        controllerType[gp] = NTT;
+        buttonCountNew = 32;
+      }
+      else {                                   // SNES Gamepad
+        if(controllerType[gp] != NTT)
+          controllerType[gp] = SNES;
+        if(buttonCountNew < 12)
+          buttonCountNew = 12;
+      }
+    }
+  }
+
+  // Set updated button count to avoid unneccesary button reads (for simpler controller types)
+  buttonCount = buttonCountNew;
+}
+
 void sendLatch()
 {
   // Send a latch pulse to (S)NES controller(s)
diff --git a/SaturnControllerUSB/SaturnControllerUSB.ino b/SaturnControllerUSB/SaturnControllerUSB.ino
index 9c08f68..dfa1693 100644
--- a/SaturnControllerUSB/SaturnControllerUSB.ino
+++ b/SaturnControllerUSB/SaturnControllerUSB.ino
@@ -24,9 +24,9 @@
 #include "Gamepad.h"
 
 #define GAMEPAD_COUNT 2 // Set to 1 or 2 depending if you want to make a 1 or 2 port adapter
-#define SELECT_PAUSE  3 // How many microseconds to wait after setting select lines? (2µs is enough according to the Saturn developer's manual)
-                        // 20µs is required for Retrobit wired controllers
-//#define RETROBIT      // Uncomment to support the Retro Bit 2.4GHz controller (this will increase lag a lot)
+#define SELECT_PAUSE 20 // How many microseconds to wait after setting select lines? (2µs is enough according to the Saturn developer's manual)
+                        // 20µs is a "safe" value that seems to work for original Saturn controllers and Retrobit wired controllers
+//#define RETROBIT_WL   // Uncomment to support the Retro Bit 2.4GHz wireless controller (this will increase lag a lot)
  
 #define UP    0x01
 #define DOWN  0x02
@@ -73,7 +73,7 @@ Saturn (P2)    Arduino Pro Micro
 9 GND         GND
 
 NOTE: The receiver of the Retro Bit 2.4GHz controller needs to be plugged
-      in after the adapter has been connected to USB and the RETROBIT
+      in after the adapter has been connected to USB and the RETROBIT_WL
       define needs to be uncommented.
 ------------------------------------------------------------------------- */
 
@@ -138,7 +138,7 @@ void loop() { while(1)
     }
   }
 
-  #ifdef RETROBIT
+  #ifdef RETROBIT_WL
     // This delay is needed for the retro bit 2.4GHz wireless controller, making it more or less useless with this adapter
     delay(17);
   #endif
diff --git a/SegaControllerUSB/SegaControllerUSB.ino b/SegaControllerUSB/SegaControllerUSB.ino
index d2081e4..fa846da 100644
--- a/SegaControllerUSB/SegaControllerUSB.ino
+++ b/SegaControllerUSB/SegaControllerUSB.ino
@@ -49,7 +49,6 @@ SegaController32U4 controller;
 
 // Set up USB HID gamepad
 Gamepad_ Gamepad;
-bool usbUpdate = false; // Should gamepad data be sent to USB?
 
 // Controller states
 word currentState = 0;
diff --git a/SegaTwoControllersUSB/SegaControllers32U4.cpp b/SegaTwoControllersUSB/SegaControllers32U4.cpp
index 1c26e21..2e6dfc6 100644
--- a/SegaTwoControllersUSB/SegaControllers32U4.cpp
+++ b/SegaTwoControllersUSB/SegaControllers32U4.cpp
@@ -38,112 +38,195 @@ SegaControllers32U4::SegaControllers32U4(void)
   PORTF |=  B11110000; // high to enable internal pull-up
   DDRB  &= ~B00001010; // input
   PORTB |=  B00001010; // high to enable internal pull-up
-  // Setup input pins (TXO,RXI,2,3,4,6 or PD3,PD2,PD1,PD0,PD4,PE6)
+  // Setup input pins (TXO,RXI,2,3,4,6 or PD3,PD2,PD1,PD0,PD4,PD7)
   DDRD  &= ~B10011111; // input
   PORTD |=  B10011111; // high to enable internal pull-up
+
+  DDRC  |= B01000000; // Select pins as output
+  DDRE  |= B01000000; 
+  PORTC |= B01000000; // Select pins high
+  PORTE |= B01000000;
   
+  _pinSelect = true;
   for(byte i=0; i<=1; i++)
   {
-    *_ddrSelect[i] |= _maskSelect[i];  // Select pins as output
-    *_portSelect[i] |= _maskSelect[i]; // Select pins high
-    _inputReg[i] = 0;
-    _currentState[i] = 0;
+    currentState[i] = 0;
     _connected[i] = 0;
     _sixButtonMode[i] = false;
     _ignoreCycles[i] = 0;
-    _pinSelect[i] = true;
   }
 }
 
-word SegaControllers32U4::getStateMD(byte gp)
+void SegaControllers32U4::readState()
 {
-  // "Normal" Six button controller reading routine, done a bit differently in this project
-  // Cycle  TH out  TR in  TL in  D3 in  D2 in  D1 in  D0 in
-  // 0      LO      Start  A      0      0      Down   Up      
-  // 1      HI      C      B      Right  Left   Down   Up
-  // 2      LO      Start  A      0      0      Down   Up      (Check connected and read Start and A in this cycle)
-  // 3      HI      C      B      Right  Left   Down   Up      (Read B, C and directions in this cycle)
-  // 4      LO      Start  A      0      0      0      0       (Check for six button controller in this cycle)
-  // 5      HI      C      B      Mode   X      Y      Z       (Read X,Y,Z and Mode in this cycle)    
-  // 6      LO      ---    ---    ---    ---    ---    ---      
-  // 7      HI      ---    ---    ---    ---    ---    ---    
-
-  // Set the select pin low/high
-  _pinSelect[gp] = !_pinSelect[gp];
-  (!_pinSelect[gp]) ? *_portSelect[gp] &= ~_maskSelect[gp] : *_portSelect[gp] |= _maskSelect[gp]; // Set LOW on even cycle, HIGH on uneven cycle
+  // Set the select pins low/high
+  _pinSelect = !_pinSelect;
+  if(!_pinSelect) {
+    PORTE &= ~B01000000;
+    PORTC &= ~B01000000;
+  } else {
+    PORTE |=  B01000000;
+    PORTC |=  B01000000;
+  }
 
   // Short delay to stabilise outputs in controller
   delayMicroseconds(SC_CYCLE_DELAY);
 
-  // Read input register(s)
-  _inputReg[0] = *_pinInputs[gp][0];
-  _inputReg[1] = *_pinInputs[gp][1];
+  // Read all input registers
+  _inputReg1 = PINF;
+  _inputReg2 = PINB;
+  _inputReg3 = PIND;
 
-  if(_ignoreCycles[gp] <= 0)
+  readPort1();
+  readPort2();
+}
+
+// "Normal" Six button controller reading routine, done a bit differently in this project
+// Cycle  TH out  TR in  TL in  D3 in  D2 in  D1 in  D0 in
+// 0      LO      Start  A      0      0      Down   Up      
+// 1      HI      C      B      Right  Left   Down   Up
+// 2      LO      Start  A      0      0      Down   Up      (Check connected and read Start and A in this cycle)
+// 3      HI      C      B      Right  Left   Down   Up      (Read B, C and directions in this cycle)
+// 4      LO      Start  A      0      0      0      0       (Check for six button controller in this cycle)
+// 5      HI      C      B      Mode   X      Y      Z       (Read X,Y,Z and Mode in this cycle)    
+// 6      LO      ---    ---    ---    ---    ---    ---      
+// 7      HI      ---    ---    ---    ---    ---    ---    
+
+void SegaControllers32U4::readPort1()
+{
+  if(_ignoreCycles[0] <= 0)
   {
-    if(_pinSelect[gp]) // Select pin is HIGH
+    if(_pinSelect) // Select pin is HIGH
     {
-      if(_connected[gp])
+      if(_connected[0])
       {
         // Check if six button mode is active
-        if(_sixButtonMode[gp])
+        if(_sixButtonMode[0])
         {
           // Read input pins for X, Y, Z, Mode
-          (bitRead(*_pinInputs[gp][SC_PIN1_BIT], _bitInputs[gp][SC_PIN1_BIT]) == LOW) ? _currentState[gp] |= SC_BTN_Z : _currentState[gp] &= ~SC_BTN_Z;
-          (bitRead(*_pinInputs[gp][SC_PIN2_BIT], _bitInputs[gp][SC_PIN2_BIT]) == LOW) ? _currentState[gp] |= SC_BTN_Y : _currentState[gp] &= ~SC_BTN_Y;
-          (bitRead(*_pinInputs[gp][SC_PIN3_BIT], _bitInputs[gp][SC_PIN3_BIT]) == LOW) ? _currentState[gp] |= SC_BTN_X : _currentState[gp] &= ~SC_BTN_X;
-          (bitRead(*_pinInputs[gp][SC_PIN4_BIT], _bitInputs[gp][SC_PIN4_BIT]) == LOW) ? _currentState[gp] |= SC_BTN_MODE : _currentState[gp] &= ~SC_BTN_MODE;
-          _sixButtonMode[gp] = false;
-          _ignoreCycles[gp] = 2; // Ignore the two next cycles (cycles 6 and 7 in table above)
+          (bitRead(_inputReg1, DB9_PIN1_BIT1) == LOW) ? currentState[0] |= SC_BTN_Z : currentState[0] &= ~SC_BTN_Z;
+          (bitRead(_inputReg1, DB9_PIN2_BIT1) == LOW) ? currentState[0] |= SC_BTN_Y : currentState[0] &= ~SC_BTN_Y;
+          (bitRead(_inputReg1, DB9_PIN3_BIT1) == LOW) ? currentState[0] |= SC_BTN_X : currentState[0] &= ~SC_BTN_X;
+          (bitRead(_inputReg1, DB9_PIN4_BIT1) == LOW) ? currentState[0] |= SC_BTN_MODE : currentState[0] &= ~SC_BTN_MODE;
+          _sixButtonMode[0] = false;
+          _ignoreCycles[0] = 2; // Ignore the two next cycles (cycles 6 and 7 in table above)
         }
         else
         {
           // Read input pins for Up, Down, Left, Right, B, C
-          (bitRead(*_pinInputs[gp][SC_PIN1_BIT], _bitInputs[gp][SC_PIN1_BIT]) == LOW) ? _currentState[gp] |= SC_BTN_UP : _currentState[gp] &= ~SC_BTN_UP;
-          (bitRead(*_pinInputs[gp][SC_PIN2_BIT], _bitInputs[gp][SC_PIN2_BIT]) == LOW) ? _currentState[gp] |= SC_BTN_DOWN : _currentState[gp] &= ~SC_BTN_DOWN;
-          (bitRead(*_pinInputs[gp][SC_PIN3_BIT], _bitInputs[gp][SC_PIN3_BIT]) == LOW) ? _currentState[gp] |= SC_BTN_LEFT : _currentState[gp] &= ~SC_BTN_LEFT;
-          (bitRead(*_pinInputs[gp][SC_PIN4_BIT], _bitInputs[gp][SC_PIN4_BIT]) == LOW) ? _currentState[gp] |= SC_BTN_RIGHT : _currentState[gp] &= ~SC_BTN_RIGHT;
-          (bitRead(*_pinInputs[gp][SC_PIN6_BIT], _bitInputs[gp][SC_PIN6_BIT]) == LOW) ? _currentState[gp] |= SC_BTN_B : _currentState[gp] &= ~SC_BTN_B;
-          (bitRead(*_pinInputs[gp][SC_PIN9_BIT], _bitInputs[gp][SC_PIN9_BIT]) == LOW) ? _currentState[gp] |= SC_BTN_C : _currentState[gp] &= ~SC_BTN_C;
+          (bitRead(_inputReg1, DB9_PIN1_BIT1) == LOW) ? currentState[0] |= SC_BTN_UP : currentState[0] &= ~SC_BTN_UP;
+          (bitRead(_inputReg1, DB9_PIN2_BIT1) == LOW) ? currentState[0] |= SC_BTN_DOWN : currentState[0] &= ~SC_BTN_DOWN;
+          (bitRead(_inputReg1, DB9_PIN3_BIT1) == LOW) ? currentState[0] |= SC_BTN_LEFT : currentState[0] &= ~SC_BTN_LEFT;
+          (bitRead(_inputReg1, DB9_PIN4_BIT1) == LOW) ? currentState[0] |= SC_BTN_RIGHT : currentState[0] &= ~SC_BTN_RIGHT;
+          (bitRead(_inputReg2, DB9_PIN6_BIT1) == LOW) ? currentState[0] |= SC_BTN_B : currentState[0] &= ~SC_BTN_B;
+          (bitRead(_inputReg2, DB9_PIN9_BIT1) == LOW) ? currentState[0] |= SC_BTN_C : currentState[0] &= ~SC_BTN_C;
         }
       }
       else // No Mega Drive controller is connected, use SMS/Atari mode
       {
         // Clear current state
-        _currentState[gp] = 0;
+        currentState[0] = 0;
         
         // Read input pins for Up, Down, Left, Right, Fire1, Fire2
-        if (bitRead(*_pinInputs[gp][SC_PIN1_BIT], _bitInputs[gp][SC_PIN1_BIT]) == LOW) { _currentState[gp] |= SC_BTN_UP; }
-        if (bitRead(*_pinInputs[gp][SC_PIN2_BIT], _bitInputs[gp][SC_PIN2_BIT]) == LOW) { _currentState[gp] |= SC_BTN_DOWN; }
-        if (bitRead(*_pinInputs[gp][SC_PIN3_BIT], _bitInputs[gp][SC_PIN3_BIT]) == LOW) { _currentState[gp] |= SC_BTN_LEFT; }
-        if (bitRead(*_pinInputs[gp][SC_PIN4_BIT], _bitInputs[gp][SC_PIN4_BIT]) == LOW) { _currentState[gp] |= SC_BTN_RIGHT; }
-        if (bitRead(*_pinInputs[gp][SC_PIN6_BIT], _bitInputs[gp][SC_PIN6_BIT]) == LOW) { _currentState[gp] |= SC_BTN_A; }
-        if (bitRead(*_pinInputs[gp][SC_PIN9_BIT], _bitInputs[gp][SC_PIN9_BIT]) == LOW) { _currentState[gp] |= SC_BTN_B; }
+        if (bitRead(_inputReg1, DB9_PIN1_BIT1) == LOW) { currentState[0] |= SC_BTN_UP; }
+        if (bitRead(_inputReg1, DB9_PIN2_BIT1) == LOW) { currentState[0] |= SC_BTN_DOWN; }
+        if (bitRead(_inputReg1, DB9_PIN3_BIT1) == LOW) { currentState[0] |= SC_BTN_LEFT; }
+        if (bitRead(_inputReg1, DB9_PIN4_BIT1) == LOW) { currentState[0] |= SC_BTN_RIGHT; }
+        if (bitRead(_inputReg2, DB9_PIN6_BIT1) == LOW) { currentState[0] |= SC_BTN_A; }
+        if (bitRead(_inputReg2, DB9_PIN9_BIT1) == LOW) { currentState[0] |= SC_BTN_B; }
       }
     }
     else // Select pin is LOW
     {
       // Check if a controller is connected
-      _connected[gp] = (bitRead(*_pinInputs[gp][SC_PIN3_BIT], _bitInputs[gp][SC_PIN3_BIT]) == LOW && bitRead(*_pinInputs[gp][SC_PIN4_BIT], _bitInputs[gp][SC_PIN4_BIT]) == LOW);
+      _connected[0] = (bitRead(_inputReg1, DB9_PIN3_BIT1) == LOW && bitRead(_inputReg1, DB9_PIN4_BIT1) == LOW);
       
       // Check for six button mode
-      _sixButtonMode[gp] = (bitRead(*_pinInputs[gp][SC_PIN1_BIT], _bitInputs[gp][SC_PIN1_BIT]) == LOW && bitRead(*_pinInputs[gp][SC_PIN2_BIT], _bitInputs[gp][SC_PIN2_BIT]) == LOW);
+      _sixButtonMode[0] = (bitRead(_inputReg1, DB9_PIN1_BIT1) == LOW && bitRead(_inputReg1, DB9_PIN2_BIT1) == LOW);
       
       // Read input pins for A and Start 
-      if(_connected[gp])
+      if(_connected[0])
       {
-        if(!_sixButtonMode[gp])
+        if(!_sixButtonMode[0])
         {
-          (bitRead(*_pinInputs[gp][SC_PIN6_BIT], _bitInputs[gp][SC_PIN6_BIT]) == LOW) ? _currentState[gp] |= SC_BTN_A : _currentState[gp] &= ~SC_BTN_A;
-          (bitRead(*_pinInputs[gp][SC_PIN9_BIT], _bitInputs[gp][SC_PIN9_BIT]) == LOW) ? _currentState[gp] |= SC_BTN_START : _currentState[gp] &= ~SC_BTN_START; 
+          (bitRead(_inputReg2, DB9_PIN6_BIT1) == LOW) ? currentState[0] |= SC_BTN_A : currentState[0] &= ~SC_BTN_A;
+          (bitRead(_inputReg2, DB9_PIN9_BIT1) == LOW) ? currentState[0] |= SC_BTN_START : currentState[0] &= ~SC_BTN_START; 
         }
       }
     }
   }
   else
   {
-    _ignoreCycles[gp]--;
+    _ignoreCycles[0]--;
   }
-
-  return _currentState[gp];
 }
+
+void SegaControllers32U4::readPort2()
+{
+  if(_ignoreCycles[1] <= 0)
+  {
+    if(_pinSelect) // Select pin is HIGH
+    {
+      if(_connected[1])
+      {
+        // Check if six button mode is active
+        if(_sixButtonMode[1])
+        {
+          // Read input pins for X, Y, Z, Mode
+          (bitRead(_inputReg3, DB9_PIN1_BIT2) == LOW) ? currentState[1] |= SC_BTN_Z : currentState[1] &= ~SC_BTN_Z;
+          (bitRead(_inputReg3, DB9_PIN2_BIT2) == LOW) ? currentState[1] |= SC_BTN_Y : currentState[1] &= ~SC_BTN_Y;
+          (bitRead(_inputReg3, DB9_PIN3_BIT2) == LOW) ? currentState[1] |= SC_BTN_X : currentState[1] &= ~SC_BTN_X;
+          (bitRead(_inputReg3, DB9_PIN4_BIT2) == LOW) ? currentState[1] |= SC_BTN_MODE : currentState[1] &= ~SC_BTN_MODE;
+          _sixButtonMode[1] = false;
+          _ignoreCycles[1] = 2; // Ignore the two next cycles (cycles 6 and 7 in table above)
+        }
+        else
+        {
+          // Read input pins for Up, Down, Left, Right, B, C
+          (bitRead(_inputReg3, DB9_PIN1_BIT2) == LOW) ? currentState[1] |= SC_BTN_UP : currentState[1] &= ~SC_BTN_UP;
+          (bitRead(_inputReg3, DB9_PIN2_BIT2) == LOW) ? currentState[1] |= SC_BTN_DOWN : currentState[1] &= ~SC_BTN_DOWN;
+          (bitRead(_inputReg3, DB9_PIN3_BIT2) == LOW) ? currentState[1] |= SC_BTN_LEFT : currentState[1] &= ~SC_BTN_LEFT;
+          (bitRead(_inputReg3, DB9_PIN4_BIT2) == LOW) ? currentState[1] |= SC_BTN_RIGHT : currentState[1] &= ~SC_BTN_RIGHT;
+          (bitRead(_inputReg3, DB9_PIN6_BIT2) == LOW) ? currentState[1] |= SC_BTN_B : currentState[1] &= ~SC_BTN_B;
+          (bitRead(_inputReg3, DB9_PIN9_BIT2) == LOW) ? currentState[1] |= SC_BTN_C : currentState[1] &= ~SC_BTN_C;
+        }
+      }
+      else // No Mega Drive controller is connected, use SMS/Atari mode
+      {
+        // Clear current state
+        currentState[1] = 0;
+        
+        // Read input pins for Up, Down, Left, Right, Fire1, Fire2
+        if (bitRead(_inputReg3, DB9_PIN1_BIT2) == LOW) { currentState[1] |= SC_BTN_UP; }
+        if (bitRead(_inputReg3, DB9_PIN2_BIT2) == LOW) { currentState[1] |= SC_BTN_DOWN; }
+        if (bitRead(_inputReg3, DB9_PIN3_BIT2) == LOW) { currentState[1] |= SC_BTN_LEFT; }
+        if (bitRead(_inputReg3, DB9_PIN4_BIT2) == LOW) { currentState[1] |= SC_BTN_RIGHT; }
+        if (bitRead(_inputReg3, DB9_PIN6_BIT2) == LOW) { currentState[1] |= SC_BTN_A; }
+        if (bitRead(_inputReg3, DB9_PIN9_BIT2) == LOW) { currentState[1] |= SC_BTN_B; }
+      }
+    }
+    else // Select pin is LOW
+    {
+      // Check if a controller is connected
+      _connected[1] = (bitRead(_inputReg3, DB9_PIN3_BIT2) == LOW && bitRead(_inputReg3, DB9_PIN4_BIT2) == LOW);
+      
+      // Check for six button mode
+      _sixButtonMode[1] = (bitRead(_inputReg3, DB9_PIN1_BIT2) == LOW && bitRead(_inputReg3, DB9_PIN2_BIT2) == LOW);
+      
+      // Read input pins for A and Start 
+      if(_connected[1])
+      {
+        if(!_sixButtonMode[1])
+        {
+          (bitRead(_inputReg3, DB9_PIN6_BIT2) == LOW) ? currentState[1] |= SC_BTN_A : currentState[1] &= ~SC_BTN_A;
+          (bitRead(_inputReg3, DB9_PIN9_BIT2) == LOW) ? currentState[1] |= SC_BTN_START : currentState[1] &= ~SC_BTN_START; 
+        }
+      }
+    }
+  }
+  else
+  {
+    _ignoreCycles[1]--;
+  }
+}
+
diff --git a/SegaTwoControllersUSB/SegaControllers32U4.h b/SegaTwoControllersUSB/SegaControllers32U4.h
index a26bf58..4260bdf 100644
--- a/SegaTwoControllersUSB/SegaControllers32U4.h
+++ b/SegaTwoControllersUSB/SegaControllers32U4.h
@@ -64,7 +64,7 @@ enum
   DB9_PIN3_BIT2 = 1,
   DB9_PIN4_BIT2 = 0,
   DB9_PIN6_BIT2 = 4,
-  DB9_PIN9_BIT2 = 6
+  DB9_PIN9_BIT2 = 7
 };
 
 const byte SC_CYCLE_DELAY = 10; // Delay (µs) between setting the select pin and reading the button pins
@@ -72,25 +72,23 @@ const byte SC_CYCLE_DELAY = 10; // Delay (µs) between setting the select pin an
 class SegaControllers32U4 {
   public:
     SegaControllers32U4(void);
-
-    word getStateMD(byte);
+    void readState();
+    word currentState[2];
 
   private:
-    word _currentState[2];
+    void readPort1();
+    void readPort2();
 
-    byte volatile * const _ddrSelect[2] =  { &DDRE, &DDRC }; 
-    byte volatile * const _portSelect[2] = { &PORTE, &PORTC }; 
-    const byte _maskSelect[2] = {B01000000, B01000000}; 
-    boolean _pinSelect[2];
+    boolean _pinSelect;
 
     byte _ignoreCycles[2];
 
     boolean _connected[2];
     boolean _sixButtonMode[2];
 
-    byte volatile * const _pinInputs[2][7] = { {&PINF,&PINF,&PINF,&PINF,&PINB,&PINB}, {&PIND,&PIND,&PIND,&PIND,&PIND,&PIND} }; 
-    byte _bitInputs[2][7] = { {7,6,5,4,3,1}, {3,2,1,0,4,7} };
-    byte _inputReg[2];
+    byte _inputReg1;
+    byte _inputReg2;
+    byte _inputReg3;
 };
 
 #endif
diff --git a/SegaTwoControllersUSB/SegaTwoControllersUSB.ino b/SegaTwoControllersUSB/SegaTwoControllersUSB.ino
index 51f4e9c..bd14777 100644
--- a/SegaTwoControllersUSB/SegaTwoControllersUSB.ino
+++ b/SegaTwoControllersUSB/SegaTwoControllersUSB.ino
@@ -57,10 +57,8 @@ SegaControllers32U4 controllers;
 
 // Set up USB HID gamepads
 Gamepad_ Gamepad[2];
-bool usbUpdate[2] = {false,false}; // Should gamepad data be sent to USB?
 
-// Controller states
-word currentState[2] = {0,0};
+// Controller previous states
 word lastState[2] = {1,1};
 
 void setup()
@@ -69,23 +67,22 @@ void setup()
     Gamepad[gp].reset();
 }
 
-void loop()
+void loop() 
 {
-  for(byte gp=0; gp<=1; gp++) {
-    currentState[gp] = controllers.getStateMD(gp);
-    sendState(gp);
-  }
+  controllers.readState();
+  sendState(0);
+  sendState(1);
 }
 
 void sendState(byte gp)
 {
   // Only report controller state if it has changed
-  if (currentState[gp] != lastState[gp])
+  if (controllers.currentState[gp] != lastState[gp])
   {
-    Gamepad[gp]._GamepadReport.buttons = currentState[gp] >> 5;
-    Gamepad[gp]._GamepadReport.Y = ((currentState[gp] & SC_BTN_DOWN) >> SC_BIT_DOWN) - ((currentState[gp] & SC_BTN_UP) >> SC_BIT_UP);
-    Gamepad[gp]._GamepadReport.X = ((currentState[gp] & SC_BTN_RIGHT) >> SC_BIT_RIGHT) - ((currentState[gp] & SC_BTN_LEFT) >> SC_BIT_LEFT);
+    Gamepad[gp]._GamepadReport.buttons = controllers.currentState[gp] >> 5;
+    Gamepad[gp]._GamepadReport.Y = ((controllers.currentState[gp] & SC_BTN_DOWN) >> SC_BIT_DOWN) - ((controllers.currentState[gp] & SC_BTN_UP) >> SC_BIT_UP);
+    Gamepad[gp]._GamepadReport.X = ((controllers.currentState[gp] & SC_BTN_RIGHT) >> SC_BIT_RIGHT) - ((controllers.currentState[gp] & SC_BTN_LEFT) >> SC_BIT_LEFT);
     Gamepad[gp].send();
-    lastState[gp] = currentState[gp];
+    lastState[gp] = controllers.currentState[gp];
   }
 }