diff --git a/src/rp2_common/pico_fix/rp2040_usb_device_enumeration/rp2040_usb_device_enumeration.c b/src/rp2_common/pico_fix/rp2040_usb_device_enumeration/rp2040_usb_device_enumeration.c
index 8319e36..2ef288e 100644
--- a/src/rp2_common/pico_fix/rp2040_usb_device_enumeration/rp2040_usb_device_enumeration.c
+++ b/src/rp2_common/pico_fix/rp2040_usb_device_enumeration/rp2040_usb_device_enumeration.c
@@ -8,6 +8,8 @@
 #include "pico/time.h"
 #include "hardware/structs/usb.h"
 #include "hardware/gpio.h"
+#include "hardware/structs/iobank0.h"
+#include "hardware/structs/padsbank0.h"
 #include "pico/fix/rp2040_usb_device_enumeration.h"
 
 #define LS_SE0 0b00
@@ -74,20 +76,37 @@ int64_t hw_enumeration_fix_force_ls_j_done(alarm_id_t id, void *user_data) {
     return 0;
 }
 
+static uint32_t gpio_ctrl_prev = 0;
+static uint32_t pad_ctrl_prev = 0;
+static const uint dp = 15;
+static const uint dm = 16;
+
 static void hw_enumeration_fix_force_ls_j(void) {
-    // Force LS_J
-    const uint dp = 15;
-    //const uint dm = 16;
-    gpio_set_function(dp, 8);
-    // TODO: assert dm is not funcseld to usb
+    // DM must be 0 for this to work. This is true if it is selected
+    // to any other function. fn 8 on this pin is only for debug so shouldn't
+    // be selected
+    if (gpio_get_function(dm) == 8) {
+        panic("Not expecting DM to be function 8");
+    }
+
+    // Before changing any pin state, take a copy of the current gpio control register
+    gpio_ctrl_prev = iobank0_hw->io[dp].ctrl;
+    // Also take a copy of the pads register
+    pad_ctrl_prev = padsbank0_hw->io[dp];
+
+    // Enable bus keep and force pin to tristate, so USB DP muxing doesn't affect
+    // pin state
+    gpio_set_pulls(dp, true, true);
+    gpio_set_oeover(dp, GPIO_OVERRIDE_LOW);
+    // Select function 8 (USB debug muxing) without disturbing other controls
+    hw_write_masked(&iobank0_hw->io[dp].ctrl,
+        8 << IO_BANK0_GPIO15_CTRL_FUNCSEL_LSB, IO_BANK0_GPIO15_CTRL_FUNCSEL_BITS);
 
     // J state is a differential 1 for a full speed device so
     // DP = 1 and DM = 0. Don't actually need to set DM low as it
     // is already gated assuming it isn't funcseld.
     gpio_set_inover(dp, GPIO_OVERRIDE_HIGH);
 
-    // TODO: What to do about existing DP state here?
-
     // Force PHY pull up to stay before switching away from the phy
     hw_set_alias(usb_hw)->phy_direct = USB_USBPHY_DIRECT_DP_PULLUP_EN_BITS;
     hw_set_alias(usb_hw)->phy_direct_override = USB_USBPHY_DIRECT_OVERRIDE_DP_PULLUP_EN_OVERRIDE_EN_BITS;
@@ -118,4 +137,9 @@ static void hw_enumeration_fix_finish(void) {
 
     // Get rid of DP pullup override
     hw_clear_alias(usb_hw)->phy_direct_override = USB_USBPHY_DIRECT_OVERRIDE_DP_PULLUP_EN_OVERRIDE_EN_BITS;
+
+    // Finally, restore the gpio ctrl value back to GPIO15
+    iobank0_hw->io[dp].ctrl = gpio_ctrl_prev;
+    // Restore the pad ctrl value
+    padsbank0_hw->io[dp] = pad_ctrl_prev;
 }