Optimised code for nes controller

teensy-nes-gamepad.c

#include <stdint.h>

// GPIO pins used for connected gamepad
#define CLOCK 21
#define LATCH 20
#define DATA  19

#define DEVICE_METHOD_JOYSTICK 0
#define DEVICE_METHOD_KEYBOARD 1

enum {
  NES_BUTTON_A,
  NES_BUTTON_B,
  NES_BUTTON_SELECT,
  NES_BUTTON_START,
  NES_DPAD_UP,
  NES_DPAD_DOWN,
  NES_DPAD_LEFT,
  NES_DPAD_RIGHT
} NES_BUTTON_MAP;

uint8_t BUTTON_STATE[16];
uint8_t DEVICE_METHOD = DEVICE_METHOD_JOYSTICK;   // Device Method (configurable)

const int16_t NES_JOYSTICK_MAP[4][2] = {
  {NES_BUTTON_A,        1},
  {NES_BUTTON_B,        2},
  {NES_BUTTON_SELECT,   7},
  {NES_BUTTON_START,    8},
};

// NES Keyboard Map (configurable)
const int16_t NES_KEYBOARD_MAP[8][2] = {
  {NES_BUTTON_A,        KEY_X},
  {NES_BUTTON_B,        KEY_Z},
  {NES_BUTTON_SELECT,   KEY_A},
  {NES_BUTTON_START,    KEY_S},
  {NES_DPAD_UP,         KEY_UP},
  {NES_DPAD_DOWN,       KEY_DOWN},
  {NES_DPAD_LEFT,       KEY_LEFT},
  {NES_DPAD_RIGHT,      KEY_RIGHT},
};

void (*PROCESS_INPUT_FN)(void);

// Delay for approx 100ns. Assumes 16Mhz AtMega CPU
#define delay100ns()
  __asm__ __volatile__ ("nop\n\t");\
  __asm__ __volatile__ ("nop\n\t");

#define SendCmdTakeSample(clock_pin, latch_pin)\
  digitalWrite(clock_pin, HIGH);\
  digitalWrite(latch_pin, HIGH);\
  delay100ns();\
  digitalWrite(latch_pin, LOW);

#define SendCmdSendData(clock_pin)\
  delay100ns();\
  digitalWrite(clock_pin, LOW);

#define SendCmdShiftOut(clock_pin)\
  delay100ns();\
  digitalWrite(clock_pin, HIGH);

void ReadInput(uint8_t *data, int num)
{
  int i;
  SendCmdTakeSample(CLOCK, LATCH);
  
  // Read output
  for (i = 0; i < num; i++) {
    SendCmdSendData(CLOCK);
    
    // 1 = Off, 0 = On... Bit must be inverted
    data[i] = (~digitalRead(DATA)) & 0x1;
    SendCmdShiftOut(CLOCK);
  }
}

int GetHatState()
{
  
  /*
  
          0
          UP
      315    45
 270 LT        RT 90
      225    135
          DN
          180
  
  */
  
  uint8_t x, y;
  const static int16_t dpad_lookup[4][4] = {
    { -1, 270,  90, -1},
    {  0, 315,  45, -1},
    {180, 225, 135, -1},
    { -1,  -1,  -1, -1}
  };
  
  y = BUTTON_STATE[NES_DPAD_UP] | (BUTTON_STATE[NES_DPAD_DOWN] << 1);
  x = BUTTON_STATE[NES_DPAD_LEFT] | (BUTTON_STATE[NES_DPAD_RIGHT] << 1);
  
  return dpad_lookup[y][x];
}

void ProcessInputNES()
{
  int i, hat0;
  
  ReadInput((uint8_t *)&BUTTON_STATE, 8);
  hat0 = GetHatState();
  
  for (i = 0; i < 4; i++) {
    Joystick.button(
      NES_JOYSTICK_MAP[i][1],
      BUTTON_STATE[NES_JOYSTICK_MAP[i][0]]);
  }
  
  Joystick.hat(hat0);
}

void ProcessInputNESUsingKeyboard()
{
  int i;
  ReadInput((uint8_t *)&BUTTON_STATE, 8);
  
  for (i = 0; i < 8; i++) {
    if (BUTTON_STATE[NES_KEYBOARD_MAP[i][0]]) {
      Keyboard.press(NES_KEYBOARD_MAP[i][1]);
    } else {
      Keyboard.release(NES_KEYBOARD_MAP[i][1]);
    }
  }
}

void ConfigureSelf()
{
  int i, d;
  
  if (DEVICE_METHOD == DEVICE_METHOD_JOYSTICK) {
    // NES, Joystick
    PROCESS_INPUT_FN = &ProcessInputNES;
  } else {
    // NES, Keybaord
    PROCESS_INPUT_FN = &ProcessInputNESUsingKeyboard;
  }
}

void setup() {
  pinMode(CLOCK, OUTPUT);
  pinMode(LATCH, OUTPUT);
  pinMode(DATA, INPUT);
  
  ConfigureSelf();
}

void loop() {
  PROCESS_INPUT_FN();
  delayMicroseconds(1);
}