diff --git a/inc/testmode.h b/inc/testmode.h
index 05580850..6ee0223d 100644
--- a/inc/testmode.h
+++ b/inc/testmode.h
@@ -13,6 +13,8 @@
 #define CMD_pins         1
 #define CMD_led          2
 #define CMD_test_headers 3
+#define CMD_wdat_osc_on  4
+#define CMD_wdat_osc_off 5
 
 /* CMD_test_headers return code in rsp.u.x[0] */
 #define TESTHEADER_success 100
diff --git a/inc/util.h b/inc/util.h
index 4f2b2c9a..d893c91f 100644
--- a/inc/util.h
+++ b/inc/util.h
@@ -104,6 +104,7 @@ void floppy_init(void);
 void floppy_process(void);
 
 /* Test mode */
+uint8_t testmode_init(void);
 void testmode_process(void);
 
 /* CRC-CCITT */
diff --git a/src/floppy.c b/src/floppy.c
index d1964949..f4e0f395 100644
--- a/src/floppy.c
+++ b/src/floppy.c
@@ -1488,8 +1488,10 @@ static void process_command(void)
         if (len != 10) goto bad_command;
         if (sig1 != 0x6e504b4e) goto bad_command;
         if (sig2 != 0x382910d3) goto bad_command;
-        floppy_state = ST_testmode;
-        break;
+        u_buf[1] = testmode_init();
+        if (u_buf[1] == ACK_OKAY)
+            floppy_state = ST_testmode;
+        goto out;
     }
 #endif
     default:
diff --git a/src/mcu/at32f4/testmode.c b/src/mcu/at32f4/testmode.c
index e08ae6ff..8c583c77 100644
--- a/src/mcu/at32f4/testmode.c
+++ b/src/mcu/at32f4/testmode.c
@@ -31,6 +31,11 @@ void testmode_get_option_bytes(void *buf)
     memcpy(buf, (void *)0x1ffff800, 32);
 }
 
+uint8_t testmode_init(void)
+{
+    return ACK_OKAY;
+}
+
 /*
  * Local variables:
  * mode: C
diff --git a/src/mcu/stm32f7/testmode.c b/src/mcu/stm32f7/testmode.c
index 7f4e1a19..d59e7662 100644
--- a/src/mcu/stm32f7/testmode.c
+++ b/src/mcu/stm32f7/testmode.c
@@ -31,6 +31,16 @@ void testmode_get_option_bytes(void *buf)
     memcpy(buf, (void *)0x1fff0000, 32);
 }
 
+uint8_t testmode_init(void)
+{
+    switch (gw_info.hw_submodel) {
+    case F7SM_lightning_plus:
+    case F7SM_v3:
+        return ACK_OKAY;
+    }
+    return ACK_BAD_COMMAND;
+}
+
 /*
  * Local variables:
  * mode: C
diff --git a/src/testmode.c b/src/testmode.c
index bbb88bfd..54e48b45 100644
--- a/src/testmode.c
+++ b/src/testmode.c
@@ -107,6 +107,37 @@ static unsigned int testmode_test_headers(void)
     return rc;
 }
 
+/* It so happens that all supported boards use the same pin and timer for 
+ * WDAT, so we can share the code here. Future boards may require this to 
+ * be made board-specific. */
+#define gpio_wdata  gpioa
+#define pin_wdata   2
+#define tim_wdata   (tim2)
+#define GPO_bus GPO_pushpull(IOSPD_LOW,HIGH)
+#define AFO_bus AFO_pushpull(IOSPD_LOW)
+static void testmode_wdat_osc_on(void)
+{
+    tim_wdata->psc = SYSCLK_MHZ/TIME_MHZ-1;
+    tim_wdata->ccmr2 = (TIM_CCMR2_CC3S(TIM_CCS_OUTPUT) |
+                        TIM_CCMR2_OC3M(TIM_OCM_PWM1));
+    tim_wdata->ccer = TIM_CCER_CC3E;
+    tim_wdata->ccr3 = time_us(1);
+    tim_wdata->arr = time_us(2)-1;
+    tim_wdata->dier = TIM_DIER_UDE;
+    tim_wdata->cr2 = 0;
+    tim_wdata->egr = TIM_EGR_UG;
+    tim_wdata->sr = 0;
+    tim_wdata->cr1 = TIM_CR1_CEN;
+    gpio_configure_pin(gpio_wdata, pin_wdata, AFO_bus);
+}
+static void testmode_wdat_osc_off(void)
+{
+    gpio_configure_pin(gpio_wdata, pin_wdata, GPO_bus);
+    tim_wdata->ccer = 0;
+    tim_wdata->cr1 = 0;
+    tim_wdata->sr = 0;
+}
+
 void testmode_process(void)
 {
     int len = ep_rx_ready(EP_RX);
@@ -141,6 +172,14 @@ void testmode_process(void)
         rsp.u.x[0] = testmode_test_headers();
         break;
     }
+    case CMD_wdat_osc_on: {
+        testmode_wdat_osc_on();
+        break;
+    }
+    case CMD_wdat_osc_off: {
+        testmode_wdat_osc_off();
+        break;
+    }
     }
 
     usb_write(EP_TX, &rsp, 32);