Move code to bootloader section and provide custom init code

stk500v2/main.cpp

@@ -3,6 +3,7 @@
 #include "uart/uart.hpp"
 
 #include <avr/boot.h>
+#include <avr/interrupt.h>
 #include <avr/pgmspace.h>
 
 #include "command.hpp"
@@ -282,7 +283,7 @@ static inline void formatErrorAnswer(Message &msg)
 
 static inline void handleMessage(Message &msg)
 {
-	static uint32_t s_address = 0;
+	static uint32_t s_address;
 
 	if (isSignOn(msg))
 		formatSignOnAnswer(msg);
@@ -328,14 +329,23 @@ int main()
 {
 	serial.init();
 
-	Message msg;
+	static Message s_msg;
 
 	while (true) {
-		if (receiveMessage(msg)) {
-			handleMessage(msg);
-			transmitMessage(msg);
+		if (receiveMessage(s_msg)) {
+			handleMessage(s_msg);
+			transmitMessage(s_msg);
 		}
 	}
 
 	return 0;
 }
+
+void startup() __attribute__((naked, section(".vectors")));
+void startup()
+{
+	asm volatile("clr __zero_reg__");
+	// SP = RAMEND;
+	SREG = 0;
+	asm volatile("jmp main");
+}