diff --git a/tools/include/atspi.h b/tools/include/atspi.h
index 1663b55..aa188bf 100644
--- a/tools/include/atspi.h
+++ b/tools/include/atspi.h
@@ -29,6 +29,7 @@ int atspi_clear_error(struct atspi_dsc *dsc);
 
 void atspi_reset(struct atspi_dsc *dsc);
 void atspi_reset_rf(struct atspi_dsc *dsc);
+int atspi_test_mode(struct atspi_dsc *dsc);
 
 void atspi_reg_write(struct atspi_dsc *dsc, uint8_t reg, uint8_t value);
 uint8_t atspi_reg_read(struct atspi_dsc *dsc, uint8_t reg);
diff --git a/tools/lib/atspi.c b/tools/lib/atspi.c
index a4ad987..023d20b 100644
--- a/tools/lib/atspi.c
+++ b/tools/lib/atspi.c
@@ -99,6 +99,15 @@ void atspi_reset_rf(struct atspi_dsc *dsc)
 }
 
 
+int atspi_test_mode(struct atspi_dsc *dsc)
+{
+	if (!dsc->driver->test_mode)
+		return 0;
+	dsc->driver->test_mode(dsc->handle);
+	return 1;
+}
+
+
 void atspi_reg_write(struct atspi_dsc *dsc, uint8_t reg, uint8_t value)
 {
 	dsc->driver->reg_write(dsc->handle, reg, value);
diff --git a/tools/lib/atusb.c b/tools/lib/atusb.c
index 070bba7..3aecb46 100644
--- a/tools/lib/atusb.c
+++ b/tools/lib/atusb.c
@@ -108,6 +108,23 @@ static void atusb_reset_rf(void *dsc)
 }
 
 
+static void atusb_test_mode(void *dsc)
+{
+	usb_dev_handle *dev = dsc;
+	int res;
+
+	if (error)
+		return;
+
+	res =
+	    usb_control_msg(dev, TO_DEV, ATSPI_TEST, 0, 0, NULL, 0, 1000);
+	if (res < 0) {
+		fprintf(stderr, "ATSPI_TEST: %d\n", res);
+		error = 1;
+	}
+}
+
+
 /* ----- register access --------------------------------------------------- */
 
 
@@ -221,6 +238,7 @@ struct atspi_driver atusb_driver = {
 	.clear_error	= atusb_clear_error,
 	.reset		= atusb_reset,
 	.reset_rf	= atusb_reset_rf,
+	.test_mode	= atusb_test_mode,
 	.reg_write	= atusb_reg_write,
 	.reg_read	= atusb_reg_read,
 	.buf_write	= atusb_buf_write,
diff --git a/tools/lib/atusd.c b/tools/lib/atusd.c
index fe2ca96..176d5de 100644
--- a/tools/lib/atusd.c
+++ b/tools/lib/atusd.c
@@ -320,6 +320,7 @@ struct atspi_driver atusd_driver = {
 	.close		= atusd_close,
 	.reset		= NULL,
 	.reset_rf	= atusd_reset_rf,
+	.test_mode	= NULL,
 	.reg_write	= atusd_reg_write,
 	.reg_read	= atusd_reg_read,
 	.buf_write	= atusd_buf_write,
diff --git a/tools/lib/driver.h b/tools/lib/driver.h
index 35e0303..d5e309b 100644
--- a/tools/lib/driver.h
+++ b/tools/lib/driver.h
@@ -25,6 +25,7 @@ struct atspi_driver {
 	int (*clear_error)(void *dsc);
 	void (*reset)(void *dsc);
 	void (*reset_rf)(void *dsc);
+	void (*test_mode)(void *dsc);
 	void (*reg_write)(void *dsc, uint8_t reg, uint8_t value);
 	uint8_t (*reg_read)(void *dsc, uint8_t reg);
 	void (*buf_write)(void *dsc, const void *buf, int size);