diff --git a/bootloader.c b/bootloader.c
index 8e59473..4d24202 100644
--- a/bootloader.c
+++ b/bootloader.c
@@ -1,5 +1,5 @@
 /*	gc_n64_usb : Gamecube or N64 controller to USB adapter firmware
-	Copyright (C) 2007-2016  Raphael Assenat <raph@raphnet.net>
+	Copyright (C) 2007-2018  Raphael Assenat <raph@raphnet.net>
 
 	This program is free software: you can redistribute it and/or modify
 	it under the terms of the GNU General Public License as published by
@@ -16,6 +16,7 @@
 */
 #include <avr/io.h>
 #include "usb.h"
+#include "bootloader.h"
 
 void enterBootLoader(void)
 {
@@ -28,5 +29,16 @@ void enterBootLoader(void)
 		"ldi r30, 0x00	\n" // ZL
 		"ldi r31, 0xF0	\n" // ZH
 		"ijmp");
-
+}
+
+void resetFirmware(void)
+{
+	usb_shutdown();
+
+	// jump to the application reset vector
+	asm volatile(
+		"cli			\n"
+		"ldi r30, 0x00	\n"
+		"ldi r31, 0x00	\n"
+		"ijmp");
 }
diff --git a/bootloader.h b/bootloader.h
index 329ade0..43f2740 100644
--- a/bootloader.h
+++ b/bootloader.h
@@ -1,2 +1,7 @@
+#ifndef _bootloader_h__
+#define _bootloader_h__
 
 void enterBootLoader(void);
+void resetFirmware(void);
+
+#endif // _bootloader_h__
diff --git a/changelog.txt b/changelog.txt
index 9014ac9..a53c3a4 100644
--- a/changelog.txt
+++ b/changelog.txt
@@ -1,3 +1,7 @@
+- ??? : Version 3.5.2
+  - Implement reset firmware command
+  - Add echo feature (USB comm test)
+
 - April 10, 2018 : Version 3.5.1
   - Fix never-stopping vibration issues (Dolphin)
 
diff --git a/hiddata.c b/hiddata.c
index 261e51c..7f30adf 100644
--- a/hiddata.c
+++ b/hiddata.c
@@ -134,9 +134,16 @@ static void hiddata_processCommandBuffer(struct hiddata_ops *ops)
 //	printf("Process cmd 0x%02x\r\n", cmdbuf[0]);
 	switch(cmdbuf[0])
 	{
+		case RQ_GCN64_ECHO:
+			// Cmd : RQ, data[]
+			// Answer: RQ, data[]
+			break;
 		case RQ_GCN64_JUMP_TO_BOOTLOADER:
 			enterBootLoader();
 			break;
+		case RQ_RNT_RESET_FIRMWARE:
+			resetFirmware();
+			break;
 		case RQ_GCN64_RAW_SI_COMMAND:
 			// TODO : Range checking
 			// cmdbuf[] : RQ, CHN, LEN, data[]
diff --git a/requests.h b/requests.h
index f5adadf..6b29747 100644
--- a/requests.h
+++ b/requests.h
@@ -2,6 +2,7 @@
 #define _gcn64_requests_h__
 
 /* Commands */
+#define RQ_GCN64_ECHO					0x00
 #define RQ_GCN64_SET_CONFIG_PARAM		0x01
 #define RQ_GCN64_GET_CONFIG_PARAM		0x02
 #define RQ_GCN64_SUSPEND_POLLING		0x03
diff --git a/tools/.gitignore b/tools/.gitignore
new file mode 100644
index 0000000..360fdba
--- /dev/null
+++ b/tools/.gitignore
@@ -0,0 +1 @@
+gcn64ctl