3 files changed, 23 insertions, 5 deletions
diff --git a/firmware/src/os/main.c b/firmware/src/os/main.c
index 973488b..b9ed4dc 100644
--- a/firmware/src/os/main.c
+++ b/firmware/src/os/main.c
@@ -45,14 +45,16 @@ int main(void)
 
 	AT91F_PIOA_CfgPMC();
 
-	/* call application specific init function */
-	_init_func();
+	wdt_init();
 
 	/* initialize USB */
 	req_ctx_init();
 	usbcmd_gen_init();
 	udp_open();
 
+	/* call application specific init function */
+	_init_func();
+
 	// Enable User Reset and set its minimal assertion to 960 us
 	AT91C_BASE_RSTC->RSTC_RMR =
 	    AT91C_RSTC_URSTEN | (0x4 << 8) | (unsigned int)(0xA5 << 24);
@@ -70,6 +72,7 @@ int main(void)
 		/* Call application specific main idle function */
 		_main_func();
 		dbgu_rb_flush();
+		wdt_restart();
 #ifdef CONFIG_IDLE
 		//cpu_idle();
 #endif
diff --git a/firmware/src/os/wdt.c b/firmware/src/os/wdt.c
index 579d15d..5a3c809 100644
--- a/firmware/src/os/wdt.c
+++ b/firmware/src/os/wdt.c
@@ -17,21 +17,31 @@
  *
  */
 
+#include <lib_AT91SAM7.h>
+#include <AT91SAM7.h>
+
+#include <os/dbgu.h>
+
 #define WDT_DEBUG
  
-void wdt_irq(void)
+static void wdt_irq(void)
 {
 	DEBUGPCRF("================> WATCHDOG EXPIRED !!!!!");
 }
 
+void wdt_restart(void)
+{
+	AT91F_WDTRestart(AT91C_BASE_WDTC);
+}
+
 void wdt_init(void)
 {
 #ifdef WDT_DEBUG
-	AT91F_WDTSetMode(AT91C_BASE_WDT, (0xfff << 16) |
+	AT91F_WDTSetMode(AT91C_BASE_WDTC, (0xfff << 16) |
 			 AT91C_WDTC_WDDBGHLT | AT91C_WDTC_WDIDLEHLT |
 			 AT91C_WDTC_WDFIEN);
 #else
-	AT91F_WDTSetMode(AT91C_BASE_WDT, (0xfff << 16) |
+	AT91F_WDTSetMode(AT91C_BASE_WDTC, (0xfff << 16) |
 			 AT91C_WDTC_WDDBGHLT | AT91C_WDTC_WDIDLEHLT |
 			 AT91C_WDTC_WDRSTEN);
 #endif
diff --git a/firmware/src/os/wdt.h b/firmware/src/os/wdt.h
new file mode 100644
index 0000000..35ea721
--- /dev/null
+++ b/firmware/src/os/wdt.h
@@ -0,0 +1,5 @@
+#ifndef _WDT_H
+extern void wdt_init(void);
+extern void wdt_restart(void);
+
+#endif