diff --git a/latency_tests/GP2040-CE_Firmware_Latency_Test_Results.xlsx b/latency_testing/GP2040-CE_Firmware_Latency_Test_Results.xlsx
similarity index 100%
rename from latency_tests/GP2040-CE_Firmware_Latency_Test_Results.xlsx
rename to latency_testing/GP2040-CE_Firmware_Latency_Test_Results.xlsx
diff --git a/latency_testing/README.md b/latency_testing/README.md
new file mode 100644
index 0000000..ac51be7
--- /dev/null
+++ b/latency_testing/README.md
@@ -0,0 +1,64 @@
+
+
+ 
+
+
+
+
+ Multi-Platform Gamepad Firmware for RP2040
+
+
+
+ Latency testing methodology and results
+
+
+
+ This section provides a transparent overview of how we do latency testing for the GP2040-CE project. It includes information about the hardware setup we use as well has how we calculate our results and copies of those results.
+
+
+
+ We encourage others to test via these methods and get in touch if they find major differences in results.
+
+
+## Setup
+
+
+ The setup used for testing is comprised of an Sunfounder Arduino MEGA 2560 with a generic USB Host Shield addon and an RP2040 Advanced Breakout Board v5.4E Passthrough Edition. The RP2040 Advanced Breakout Board v5.4E Passthrough Edition is connected to the USB Host Shield addon with a 3.3' Ugreen USB-C to USB-A cable. The USB Host Shield addon is connected to the Ardunio MEGA 2560 directly via available header stack. The Arduino MEGA 2560 is connected to an M1 MacBook Air with a 3' Ugreen USB-C to USB-B cable. A 1' female to female Depont wire is connected between pin 7 on the USB Host Shield and pin 19 on the RP2040 Advanced Breakout Board v5.4E Passthrough Edition's 20 pin header which is the `up` direction by default. No other wires or cables are needed.
+
+
+
+ Serial monitoring can be done via a number of programs. I use `CoolTerm` for serial monitoring on the M1 MacBook Air because it allows me to copy and past result sets easily.
+
+
+
+ Links to everything mentioned above can be found below:
+
+
+* Arduino MEGA 2560 [Link](https://www.amazon.com/s?k=arduino+mega+2560&crid=2J2XP0ONNYGL2&sprefix=arduino+mega+2560%2Caps%2C96&ref=nb_sb_noss_1)
+* USB Host Shield [Link](https://www.amazon.com/s?k=arduino+host+shield&crid=3LKKHMBPUVXLS&sprefix=arduino+host+shield%2Caps%2C82&ref=nb_sb_noss_1
+* RP2040 Advanced Breakout Board v5.4E Passthrough Edition [Link](https://github.com/OpenStickCommunity/Hardware/tree/main/RP2040%20Advanced%20Breakout%20Board%20-%20Passthrough)
+* USB-C to USB-A 3.3' cable [Link](https://a.co/d/2USECGQ)
+* USB-C to USB-B 3' cable [Link](https://a.co/d/7vAX8nV)
+* Female to female Dupont wire [Link](https://www.amazon.com/s?k=dupont+cable&crid=TZGT87HBYSIS&sprefix=dupont+cable%2Caps%2C138&ref=nb_sb_noss_1)
+
+## Arduino Sketch
+
+
+ A copy of the optamized USB Lag sketch we use can be found [HERE](https://raw.githubusercontent.com/OpenStickCommunity/GP2040-CE/main/site/latency_testing/usblag_optimized.ino) in our latency_testing folder directly, or can be downloaded from FeralAI's original repo [HERE](https://github.com/FeralAI/usblag_optimized?tab=readme-ov-file). We follow the methodology and setup outlined by inputlag.science which can be found [HERE](https://inputlag.science/controller/methodology).
+
+
+
+ The changes in this optamized version of the USB Lag sketch are:
+ * Tuned core input loop logic to reduce unnecessary overhead - Test results will be around ~0.1ms faster than the original sketch, and very close the results from the MiSTer Input Latency Tester [LINK](https://github.com/misteraddons/inputlatency)
+ * See test results immediately after test by pressing =
+
+
+## Result collection methodology
+
+
+ By default the optamized USB Lag sketch will run a series of 1,000 inputs and measure their latency. We have found through testing that there can be very small variances in results when comparing mutiple 1,000 input tests together. As such, we run the optamized USB Lag sketch 10 times in a row and average the averages of each 1,000 individual tests to come up with our average latency. It is important to note that we still look at the overall highest latency input as our high number, and the lowest overall low latency input as our low number. We do not average these two results.
+
+
+
+ It is also important to note that these 10 runs are always conducted back-to-back. We do not cherry pick good runs which could lead to a deflated and unrealistic overall average latency number.
+
\ No newline at end of file
diff --git a/latency_testing/usblag_optimized.ino b/latency_testing/usblag_optimized.ino
new file mode 100644
index 0000000..3358b36
--- /dev/null
+++ b/latency_testing/usblag_optimized.ino
@@ -0,0 +1,434 @@
+#define BUTTON_PIN 7
+
+#define ENABLE_BT 0
+#include "HIDDriver.h"
+#include "XInputDriver.h"
+#if ENABLE_BT
+#include
+#endif
+#include "HIDReportMask.h"
+
+#include "desc.h"
+
+float min = 0.0;
+float max = 0.0;
+float sumx = 0.0;
+float sumx2 = 0.0;
+
+unsigned long nextchange;
+unsigned long time;
+int total = 5000;
+byte state = 0;
+byte button = 0;
+USB Usb;
+HIDReportMask report_mask;
+
+#define BUFFER_SIZE 64
+
+#define PS4_VID 0x054C // Sony Corporation
+#define PS4_PID 0x05C4 // PS4 Controller
+#define PS4_PID_SLIM 0x09CC // PS4 Slim Controller
+
+uint8_t intervalPow2(uint8_t interval)
+{
+ if (interval <= 2)
+ {
+ return interval;
+ }
+ if (interval < 4)
+ {
+ return 2;
+ }
+ if (interval < 8)
+ {
+ return 4;
+ }
+ if (interval < 16)
+ {
+ return 8;
+ }
+ if (interval < 32)
+ {
+ return 16;
+ }
+ return 32;
+}
+
+uint8_t pollBuffer[BUFFER_SIZE];
+uint8_t prevBuffer[BUFFER_SIZE];
+
+class TimingManager
+{
+public:
+ EpInfo *pollingEP = 0;
+ uint8_t address = 0;
+ USB *usbRef;
+ uint8_t pollingInterval = 0;
+ uint32_t nextPollingTime = 0;
+ bool doPoll = false;
+ uint8_t overrideInterval = 0;
+ TimingManager(USB *usb) : usbRef(usb) {}
+ void doInit(uint8_t interval, uint8_t bAddress, EpInfo *ep)
+ {
+ pollingInterval = interval;
+ pollingEP = ep;
+ doPoll = true;
+ overrideInterval = 0;
+ address = bAddress;
+ nextPollingTime = 0;
+ memset(prevBuffer, 0, BUFFER_SIZE);
+ Serial.print(F("Interval:\t"));
+ Serial.print(interval);
+ Serial.println(F("ms"));
+ }
+ bool canPoll()
+ {
+ if (!doPoll || !pollingEP)
+ return false;
+ // do burn
+ if (overrideInterval == 255)
+ return true;
+ if ((int32_t)((uint32_t)millis() - nextPollingTime) >= 0L)
+ {
+ nextPollingTime = (uint32_t)millis() + (overrideInterval > 0 ? overrideInterval : pollingInterval);
+ return true;
+ }
+ return false;
+ }
+ uint8_t pollDevice()
+ {
+ if (!canPoll())
+ return 0;
+
+ // Serial.println("Coincoin");
+ memset(pollBuffer, 0, BUFFER_SIZE);
+ uint16_t buffer_size = pollingEP->maxPktSize;
+ uint8_t res = usbRef->inTransfer(address, pollingEP->epAddr, &buffer_size, pollBuffer);
+ if (res != 0 && res != hrNAK)
+ {
+ Serial.print(F("Cant poll device "));
+ Serial.print(res);
+ Serial.print("\n");
+ }
+ if (res == hrNAK)
+ {
+ return 0;
+ }
+ return doMeasure(pollingEP->maxPktSize, pollBuffer);
+ }
+ uint8_t doMeasure(uint8_t len, uint8_t *buf)
+ {
+ float diff = (micros() - time) / 1000.0;
+ report_mask.apply_mask(len, buf);
+ if (memcmp(buf, prevBuffer, len) != 0)
+ {
+ sumx += diff;
+ sumx2 += (diff * diff);
+ if (diff >= max)
+ {
+ max = diff;
+ }
+ if (diff <= min || min == 0.0)
+ {
+ min = diff;
+ }
+ Serial.println(diff);
+ /*for (uint8_t i = 0 ; i < len; ++i) {
+ print_hex(buf[i], 8);
+ }
+ Serial.println();
+ */
+ memcpy(prevBuffer, buf, len);
+ time = 0;
+ }
+ return 0;
+ }
+};
+
+#if ENABLE_BT
+class BTController : public BTHID, public TimingManager
+{
+public:
+ BTController(BTD *p, bool pair = false, const char *pin = "0000") : BTHID(p, pair, pin), TimingManager(NULL)
+ {
+ }
+ virtual void OnInitBTHID()
+ {
+ TimingManager::doInit(0, 0, NULL);
+ enable_sixaxis();
+ Serial.print(F("BTHID Controller initialized\n"));
+ report_mask.do_mask = false;
+ }
+ virtual void ParseBTHIDData(uint8_t len, uint8_t *buf)
+ {
+ doMeasure(10, buf + 2);
+ }
+ void enable_sixaxis(){
+ // Command used to make the PS4 controller send out the entire output report
+ /* {
+ uint8_t buf[2];
+ buf[0] = 0x90; // HID BT Get_report (0x40) | Report Type (Feature 0x03)
+ buf[1] = 0x00; // Report ID
+
+ HID_Command(buf, 2);
+ }
+ delay(150);
+ {
+ uint8_t buf[79];
+ memset(buf, 0, sizeof(buf));
+
+ buf[0] = 0x52; // HID BT Set_report (0x50) | Report Type (Output 0x02)
+ buf[1] = 0x11; // Report ID
+ buf[2] = 0x80;
+ buf[4]= 0xFF;
+
+ buf[7] = 0; // Small Rumble
+ buf[8] = 0; // Big rumble
+
+ buf[9] = 0xFF; // Red
+ buf[10] = 0xFF; // Green
+ buf[11] = 0xFF; // Blue
+
+ buf[12] = 0; // Time to flash bright (255 = 2.5 seconds)
+ buf[13] = 0; // Time to flash dark (255 = 2.5 seconds)
+
+ // The PS4 console actually set the four last bytes to a CRC32 checksum, but it seems like it is actually not needed
+ buf[75] = 0xFE;
+ buf[76] = 0x84;
+ buf[77] = 0xA3;
+ buf[78] = 0x79;
+ HID_Command(buf, sizeof(buf));
+ }*//*{
+
+ uint8_t buf[] = {0x53, 0x03, 0x02, 0x00, 0xf1, 0xdf, 0xd3, 0x7b, 0x4f, 0x49, 0x0b, 0x0b, 0x7c, 0x79, 0xde, 0xad,
+0x5d, 0xa3, 0x41, 0x8a, 0x9c, 0x2e, 0xaf, 0x09, 0xc4, 0xa6, 0x80, 0xb4, 0x82, 0x87, 0x2c, 0xbf,
+0x86, 0xe0, 0x2a, 0x86, 0x60, 0xa0, 0x23, 0x33};
+ //memset(buf, 0, sizeof(buf));
+
+ HID_Command(buf, sizeof(buf));
+ }*/
+ };
+
+ void HID_Command(uint8_t *data, uint8_t nbytes)
+ {
+ pBtd->L2CAP_Command(hci_handle, data, nbytes, control_scid[0], control_scid[1]);
+ };
+};
+#endif
+
+class XBoxController : public XInputDriver, public TimingManager
+{
+public:
+ XBoxController(USB *usb) : XInputDriver(usb), TimingManager(usb) {}
+ uint8_t Init(uint8_t parent, uint8_t port, bool lowspeed)
+ {
+ uint8_t res = XInputDriver::Init(parent, port, lowspeed);
+ if (res)
+ return res;
+ TimingManager::doInit(bInterval, bAddress, &epInfo[XBOX_INPUT_PIPE]);
+ if (isXboxOne)
+ {
+ Serial.print(F("XBOX One Controller initialized\n"));
+
+ startDevice();
+
+ // initRumble();
+ }
+ else
+ {
+ Serial.print(F("XBOX Controller initialized\n"));
+ }
+ report_mask.interesting_bit_idx = 2 * 8;
+ for (int i = 0; i < report_mask.interesting_bit_idx; i++)
+ {
+ report_mask.interesting_bits[i] = 16 + i;
+ }
+ report_mask.do_mask = true;
+ return 0;
+ }
+ uint8_t Release()
+ {
+ XInputDriver::Release();
+ TimingManager::doPoll = false;
+ return 0;
+ }
+ uint8_t Poll() { return pollDevice(); }
+};
+
+class HIDController : public HIDDriver, public TimingManager
+{
+public:
+ HIDController(USB *usb) : HIDDriver(usb), TimingManager(usb) {}
+ uint8_t Init(uint8_t parent, uint8_t port, bool lowspeed)
+ {
+ uint8_t res = HIDDriver::Init(parent, port, lowspeed);
+ if (res)
+ return res;
+ TimingManager::doInit(pollInterval, bAddress, &epInfo[hidInterfaces[0].epIndex[epInterruptInIndex]]);
+
+ if (GetReportDescr(0, &report_mask))
+ {
+ Serial.println(F("Cannot read HID report"));
+ }
+ if (bNumIface > 1)
+ {
+ Serial.println(F("Warning! Multi-channel HID device not supported! Selecting only the first one."));
+ }
+ Serial.print(F("HID Controller initialized\n"));
+ return 0;
+ }
+ uint8_t Release()
+ {
+ HIDDriver::Release();
+ TimingManager::doPoll = false;
+ return 0;
+ }
+ uint8_t Poll() { return pollDevice(); }
+};
+
+HIDController Hid(&Usb);
+XBoxController xbox(&Usb);
+
+#if ENABLE_BT
+
+BTD Btd(&Usb); // You have to create the Bluetooth Dongle instance like so
+
+/* You can create the instance of the PS4BT class in two ways */
+// This will start an inquiry and then pair with the PS4 controller - you only have to do this once
+// You will need to hold down the PS and Share button at the same time, the PS4 controller will then start to blink rapidly indicating that it is in pairing mode
+BTController btHid(&Btd);
+
+#endif
+
+void setup()
+{
+
+ pinMode(BUTTON_PIN, OUTPUT);
+ digitalWrite(BUTTON_PIN, LOW);
+ Serial.begin(115200);
+#if !defined(__MIPSEL__)
+ while (!Serial)
+ ; // Wait for serial port to connect - used on Leonardo, Teensy and other boards with built-in USB CDC serial connection
+#endif
+ Serial.println(F("Start"));
+
+ if (Usb.Init() == -1)
+ Serial.println(F("OSC did not start."));
+
+ randomSeed(analogRead(0));
+ delay(200);
+
+ nextchange = micros() + uint32_t(5000) * 1000;
+}
+// int looptime = 0;
+void loop()
+{
+ // Check inputs at beginning of loop
+ Usb.Task();
+
+ if (total < 1000)
+ {
+ if (micros() >= nextchange)
+ {
+ if (time != 0)
+ {
+ Serial.println(F("Input was dropped!"));
+ }
+ button = !button;
+ digitalWrite(BUTTON_PIN, button);
+ time = micros();
+ total++;
+ nextchange = micros() + random(50, 70) * 1000 + random(0, 250) * 4;
+ }
+ }
+ else if (Serial.available())
+ {
+ // Serial.println(F("Waiting for input..."));
+ byte inByte = Serial.read();
+ uint8_t reading_interval;
+ switch (inByte)
+ {
+ case '1':
+ button = true;
+ digitalWrite(BUTTON_PIN, button);
+ Serial.println(F("High"));
+ time = micros();
+ break;
+ case '0':
+ button = false;
+ digitalWrite(BUTTON_PIN, button);
+ Serial.println(F("Low"));
+ time = micros();
+ break;
+ case 'b':
+ Hid.overrideInterval = 255;
+ xbox.overrideInterval = 255;
+ Serial.println(F("Switch to burn mode: disabling polling interval"));
+ break;
+ case 'o':
+ Hid.overrideInterval = 1;
+ xbox.overrideInterval = 1;
+ Serial.println(F("Override polling interval to 1ms!"));
+ break;
+ case '2':
+ Hid.overrideInterval = 2;
+ xbox.overrideInterval = 2;
+ Serial.println(F("Override polling interval to 2ms!"));
+ break;
+ case '4':
+ Hid.overrideInterval = 4;
+ xbox.overrideInterval = 4;
+ Serial.println(F("Override polling interval to 4ms!"));
+ break;
+ case '8':
+ Hid.overrideInterval = 8;
+ xbox.overrideInterval = 8;
+ Serial.println(F("Override polling interval to 8ms!"));
+ break;
+ case 'n':
+ Hid.overrideInterval = 0;
+ xbox.overrideInterval = 0;
+ Serial.println(F("Back to normal polling interval!"));
+ break;
+ case 'p':
+ reading_interval = intervalPow2(Hid.pollingInterval ? Hid.pollingInterval : xbox.pollingInterval);
+ Hid.overrideInterval = reading_interval;
+ xbox.overrideInterval = reading_interval;
+ Serial.print(F("Override polling interval to "));
+ Serial.print(reading_interval);
+ Serial.print(F("ms!"));
+ break;
+ case 't':
+ min = 0.0;
+ max = 0.0;
+ sumx = 0.0;
+ sumx2 = 0.0;
+ button = false;
+ total = 0;
+ digitalWrite(BUTTON_PIN, button);
+ time = 0;
+ Serial.println(F("Launching test"));
+ randomSeed(analogRead(0));
+ nextchange = micros() + random(50, 70) * 1000 + random(0, 250) * 4;
+ break;
+ case 's':
+ reading_interval = Serial.parseInt();
+ Hid.overrideInterval = reading_interval;
+ xbox.overrideInterval = reading_interval;
+ Serial.print(F("Override polling to "));
+ Serial.print(reading_interval);
+ Serial.println(F("ms!"));
+ break;
+ case '=':
+ Serial.print(F("min: "));
+ Serial.println(min);
+ Serial.print(F("max: "));
+ Serial.println(max);
+ Serial.print(F("average: "));
+ Serial.println(sumx / 1000.0);
+ Serial.print(F("stddev: "));
+ Serial.println(sqrt(((1000.0 * sumx2) - (sumx * sumx)) / (1000.0 * (1000.0 - 1.0))));
+ break;
+ }
+ }
+}