From ac0a51399fea5e3b0f3d8489b0c1319b216cf434 Mon Sep 17 00:00:00 2001
From: Fabrice Desclaux <serpilliere@droids-corp.org>
Date: Thu, 29 Mar 2012 19:36:14 +0200
Subject: [PATCH] fix bootloader for atm2560

---
 main.c | 21 ++++++++++++++++-----
 1 file changed, 16 insertions(+), 5 deletions(-)

diff --git a/main.c b/main.c
index ad94ba8..5f7a638 100644
--- a/main.c
+++ b/main.c
@@ -499,24 +499,35 @@ void xbee_unload_timeout(struct xbee_ctx *ctx)
 
 void bootloader(void)
 {
-#define BOOTLOADER_ADDR 0x1f800
+#ifndef USE_USB
+#define BOOTLOADER_ADDR 0x3f000
 	if (pgm_read_byte_far(BOOTLOADER_ADDR) == 0xff) {
 		printf_P(PSTR("Bootloader is not present\r\n"));
 		return;
 	}
 	cli();
 	/* ... very specific :( */
+	TIMSK0 = 0;
+	TIMSK1 = 0;
+	TIMSK2 = 0;
+	TIMSK3 = 0;
+	TIMSK4 = 0;
+	TIMSK5 = 0;
 	EIMSK = 0;
+	UCSR0B = 0;
+	UCSR1B = 0;
+	UCSR2B = 0;
+	UCSR3B = 0;
 	SPCR = 0;
 	TWCR = 0;
 	ACSR = 0;
 	ADCSRA = 0;
 
-	__asm__ __volatile__ ("ldi r31,0xf0\n");
+	EIND = 1;
+	__asm__ __volatile__ ("ldi r31,0xf8\n");
 	__asm__ __volatile__ ("ldi r30,0x00\n");
-	__asm__ __volatile__ ("ijmp\n");
-
-	/* never returns */
+	__asm__ __volatile__ ("eijmp\n");
+#endif
 }
 
 void xbee_mainloop(void)
-- 
2.39.5