diff --git a/bootloader/bootloader.c b/bootloader/bootloader.c
index ae6e244..a3c969d 100644
--- a/bootloader/bootloader.c
+++ b/bootloader/bootloader.c
@@ -96,6 +96,7 @@ uint8_t pageBuf[APP_SECTION_PAGE_SIZE];
 
 void pollEndpoint(void);
 
+
 /// Configure the device for bootloader mode and loop responding to bootloader commands
 void runBootloader(void){
 	// Turn on LED
@@ -145,6 +146,16 @@ int main(void){
 	
 	_delay_us(100);
 
+	PORTA.DIRSET = PIN5_bm | PIN6_bm;
+	PORTA.OUTCLR = PIN5_bm | PIN6_bm;
+
+	/* asm volatile("ldi r22, %0"::"i" (0x00)); */
+	/* asm volatile("ldi r23, %0"::"i" (0x70)); */
+	/* asm volatile("ldi r24, %0"::"i" (0x00)); */
+	/* asm volatile("ldi r25, %0"::"i" (0x00)); */
+	/* asm volatile("call 0x8C00"::: "r18", "r19", "r20", "r21", "r22", "r23", */
+	/* 		"r24", "r25", "r26", "r27", "r30", "r31"); */
+
 	if (!(CHECK_PORT.IN & (1<<CHECK_PIN)) // If the specified pin is pulled LOW
 	|| pgm_read_word(0) == 0xFFFF // Get the value of the reset vector. If it's unprogrammed, we know
 	                               // there's nothing useful in app flash
diff --git a/bootloader/makefile b/bootloader/makefile
index 7f914c6..44f0178 100644
--- a/bootloader/makefile
+++ b/bootloader/makefile
@@ -65,13 +65,13 @@ CPPSRC =
 #     Even though the DOS/Win* filesystem matches both .s and .S the same,
 #     it will preserve the spelling of the filenames, and gcc itself does
 #     care about how the name is spelled on its command-line.
-ASRC = sp_driver.S
+ASRC = sp_driver.S mine.S
 
 
 # Optimization level, can be [0, 1, 2, 3, s].
 #     0 = turn off optimization. s = optimize for size.
 #     (Note: 3 is not always the best optimization level. See avr-libc FAQ.)
-OPT = 3
+OPT = s
 
 # List any extra directories to look for include files here.
 #     Each directory must be seperated by a space.
@@ -228,13 +228,16 @@ endif
 #    --cref:    add cross reference to  map file
 LDFLAGS  = -Wl,-Map=$(TARGET).map,--cref
 LDFLAGS += -Wl,--relax
-LDFLAGS += -Wl,--gc-sections
+# LDFLAGS += -Wl,--gc-sections
+# LDFLAGS += -Wl,-T./avrxmega2.xn
 LDFLAGS += $(EXTMEMOPTS)
 LDFLAGS += $(patsubst %,-L%,$(EXTRALIBDIRS))
 LDFLAGS += $(PRINTF_LIB) $(SCANF_LIB) $(MATH_LIB)
+LDFLAGS += -Wl,--section-start=.test_data=0x007000
+LDFLAGS += -Wl,--section-start=.mytest=0x008C00
 LDFLAGS += -Wl,--section-start=.text=$(BOOT_SECTION_START)
 LDFLAGS += -Wl,--section-start=.noinit=0x802400 # magic flag for bootloader entry
-LDFLAGS += -Wl,--section-start=.boot-entry=$(shell python -c "print hex($(BOOT_SECTION_START)+4096-8)")
+LDFLAGS += -Wl,--section-start=.boot-entry=$(shell python -c "print(hex($(BOOT_SECTION_START)+4096-8))")
 
 #---------------- Programming Options (avrdude) ----------------
 
@@ -247,7 +250,7 @@ AVRDUDE_PROGRAMMER = avrispmkii
 # com1 = serial port. Use lpt1 to connect to parallel port.
 AVRDUDE_PORT = usb
 
-AVRDUDE_WRITE_FLASH = -e -U boot:w:$(TARGET).hex
+AVRDUDE_WRITE_FLASH = -e -U flash:w:$(TARGET).hex
 #AVRDUDE_WRITE_EEPROM = -U eeprom:w:$(TARGET).eep
 
 
@@ -364,16 +367,16 @@ sizeafter:
 
 
 # Program the device.
-program: $(TARGET).hex $(TARGET).eep
-	$(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM)
-
-REPOSITION_BOOTLOADER += --change-addresses -$(BOOT_SECTION_START)
+# program: $(TARGET).hex $(TARGET).eep
+# 	$(AVRDUDE) $(AVRDUDE_FLAGS) $(AVRDUDE_WRITE_FLASH) $(AVRDUDE_WRITE_EEPROM)
+program: $(TARGET).hex
+	avrdude-pdi -C ~/local/etc/avrdude-pdi.conf -c usbasp -p x32a4 -U flash:w:$(TARGET).hex:i -E noreset -e
 
 # Create final output files (.hex, .eep) from ELF output file.
 %.hex: %.elf
 	@echo
 	@echo $(MSG_FLASH) $@
-	$(OBJCOPY) $(REPOSITION_BOOTLOADER) -O $(FORMAT) -R .eeprom -R .fuse -R .lock $< $@
+	$(OBJCOPY) -O $(FORMAT) -R .eeprom -R .fuse -R .lock $< $@
 
 %.eep: %.elf
 	@echo
diff --git a/bootloader/mine.S b/bootloader/mine.S
new file mode 100644
index 0000000..44c6256
--- /dev/null
+++ b/bootloader/mine.S
@@ -0,0 +1,170 @@
+.section .mytest,"ax",@progbits
+
+.global my_test_asm
+
+__SP_H__ = 0x3e
+__SP_L__ = 0x3d
+
+; my_test_asm:
+my_test_asm:
+	push r28	 ;
+	push r29	 ;
+	in r28,__SP_L__	 ;
+	in r29,__SP_H__	 ;
+.L__stack_usage = 2
+	rcall SP_EraseApplicationPage_2
+	rcall SP_WaitForSPM_2
+
+; turn on the led
+	ldi r24,0	 ;  _1
+	ldi r25,lo8(6)	 ;  _1,
+	ldi r18,lo8(32)	 ;  tmp43,
+	movw r30,r24	 ; , _1
+	std Z+5,r18	 ;  _1->OUTSET, tmp43
+	nop
+	nop
+	pop r29	 ;
+	pop r28	 ;
+	ret
+
+
+	; push r28	 ;
+	; push r29	 ;
+	; in r28,__SP_L__	 ;
+	; in r29,__SP_H__	 ;
+; /* prologue: function */
+; /* frame size = 0 */
+; /* stack size = 2 */
+; .L__stack_usage = 2
+	; ldi r24,0	 ;  _1
+	; ldi r25,lo8(6)	 ;  _1,
+	; ldi r18,lo8(32)	 ;  tmp43,
+	; movw r30,r24	 ; , _1
+	; std Z+5,r18	 ;  _1->OUTSET, tmp43
+	; nop
+; /* epilogue start */
+	; pop r29	 ;
+	; pop r28	 ;
+	; ret
+
+#include <avr/io.h>
+;#include "Flash_Defines.h"
+
+/* Define the size of the flash page if not defined in the header files. */
+#ifndef APP_SECTION_PAGE_SIZE
+	#error  APP_SECTION_PAGE_SIZE must be defined if not defined in header files.
+	//#define APP_SECTION_PAGE_SIZE 512
+#endif /*APP_SECTION_PAGE_SIZE*/
+
+/* Defines not yet included in header file. */
+#define NVM_CMD_NO_OPERATION_gc (0x00<<0)	// Noop/Ordinary LPM
+#define NVM_CMD_READ_USER_SIG_ROW_gc (0x01<<0)	// Read user signature row
+#define NVM_CMD_READ_CALIB_ROW_gc (0x02<<0)	// Read calibration row
+#define NVM_CMD_READ_EEPROM_gc (0x06<<0)	// Read EEPROM
+#define NVM_CMD_READ_FUSES_gc (0x07<<0)	// Read fuse byte
+#define NVM_CMD_WRITE_LOCK_BITS_gc (0x08<<0)	// Write lock bits
+#define NVM_CMD_ERASE_USER_SIG_ROW_gc (0x18<<0)	// Erase user signature row
+#define NVM_CMD_WRITE_USER_SIG_ROW_gc (0x1A<<0)	// Write user signature row
+#define NVM_CMD_ERASE_APP_gc (0x20<<0)	// Erase Application Section
+#define NVM_CMD_ERASE_APP_PAGE_gc (0x22<<0)	// Erase Application Section page
+#define NVM_CMD_LOAD_FLASH_BUFFER_gc (0x23<<0)	// Load Flash page buffer
+#define NVM_CMD_WRITE_APP_PAGE_gc (0x24<<0)	// Write Application Section page
+#define NVM_CMD_ERASE_WRITE_APP_PAGE_gc (0x25<<0)	// Erase-and-write Application Section page
+#define NVM_CMD_ERASE_FLASH_BUFFER_gc (0x26<<0)	// Erase/flush Flash page buffer
+#define NVM_CMD_ERASE_BOOT_PAGE_gc (0x2A<<0)	// Erase Boot Section page
+#define NVM_CMD_WRITE_BOOT_PAGE_gc (0x2C<<0)	// Write Boot Section page
+#define NVM_CMD_ERASE_WRITE_BOOT_PAGE_gc (0x2D<<0)	// Erase-and-write Boot Section page
+#define NVM_CMD_ERASE_EEPROM_gc (0x30<<0)	// Erase EEPROM
+#define NVM_CMD_ERASE_EEPROM_PAGE_gc (0x32<<0)	// Erase EEPROM page
+#define NVM_CMD_LOAD_EEPROM_BUFFER_gc (0x33<<0)	// Load EEPROM page buffer
+#define NVM_CMD_WRITE_EEPROM_PAGE_gc (0x34<<0)	// Write EEPROM page
+#define NVM_CMD_ERASE_WRITE_EEPROM_PAGE_gc (0x35<<0)	// Erase-and-write EEPROM page
+#define NVM_CMD_ERASE_EEPROM_BUFFER_gc (0x36<<0)	// Erase/flush EEPROM page buffer
+#define NVM_CMD_APP_CRC_gc (0x38<<0)	// Generate Application section CRC
+#define NVM_CMD_BOOT_CRC_gc (0x39<<0)	// Generate Boot Section CRC
+#define NVM_CMD_FLASH_RANGE_CRC_gc (0x3A<<0)	// Generate Flash Range CRC
+#define CCP_SPM_gc (0x9D<<0)	// SPM Instruction Protection
+#define CCP_IOREG_gc (0xD8<<0)	// IO Register Protection
+
+
+; ---
+; This routine erases the page at address R25:R24:R23:R22 in the application
+; section. The address can point anywhere inside the page.
+;
+; Input:
+;     R25:R24:R23:R22 - Byte address into Flash page.
+;
+; Returns:
+;     Nothing.
+; ---
+
+SP_EraseApplicationPage_2:
+	in	r19, RAMPZ                      ; Save RAMPZ, which is restored in SP_CommonSPM.
+	out	RAMPZ, r24                      ; Load RAMPZ with the MSB of the address.
+	movw    r24, r22                        ; Move low bytes for ZH:ZL to R25:R24
+	ldi	r20, NVM_CMD_ERASE_APP_PAGE_gc  ; Prepare NVM command in R20.
+	jmp	SP_CommonSPM_2                    ; Jump to common SPM code.
+
+
+; This routine wait for the SPM to finish and clears the command register.
+;
+; Note that this routine is blocking, and will halt any execution until the SPM
+; is finished.
+;
+; Input:
+;     Nothing.
+;
+; Returns:
+;     Nothing.
+; ---
+
+SP_WaitForSPM_2:
+	lds	r18, NVM_STATUS     ; Load the NVM Status register.
+	sbrc	r18, NVM_NVMBUSY_bp ; Check if bit is cleared.
+	rjmp	SP_WaitForSPM_2     ; Repeat check if bit is not cleared.
+	clr	r18
+	sts	NVM_CMD, r18        ; Clear up command register to NO_OPERATION.
+	ret
+
+; ---
+; This routine is called by several other routines, and contains common code
+; for executing an SPM command, including the return statement itself.
+;
+; If the operation (SPM command) requires the R1:R0 registers to be
+; prepared, this must be done before jumping to this routine.
+;
+; Note that you must define "-Wl,--section-start=.BOOT=0x020000" for the
+; linker to place this function in the boot section with the correct address.
+;
+; Input:
+;     R1:R0    - Optional input to SPM command.
+;     R25:R24  - Low bytes of Z pointer.
+;     R20      - NVM Command code.
+;
+; Returns:
+;     Nothing.
+; ---
+
+SP_CommonSPM_2:
+	movw	ZL, r24          ; Load R25:R24 into Z.
+	sts	NVM_CMD, r20     ; Load prepared command into NVM Command register.
+	ldi	r18, CCP_SPM_gc  ; Prepare Protect SPM signature in R18
+	sts	CCP, r18         ; Enable SPM operation (this disables interrupts for 4 cycles).
+	spm                      ; Self-program.
+	clr	r1               ; Clear R1 for GCC _zero_reg_ to function properly.
+	out	RAMPZ, r19       ; Restore RAMPZ register.
+	ret
+
+.section .test_data,"a",@progbits
+	.dc.b 0x00
+	.dc.b 0x11
+	.dc.b 0x00
+	.dc.b 0x11
+	.dc.b 0x00
+	.dc.b 0x11
+	.dc.b 0x00
+	.dc.b 0x11
+	.dc.b 0x00
+	.dc.b 0x11
+	.dc.b 0x00
+	.dc.b 0x11
diff --git a/bootloader/sp_driver.S b/bootloader/sp_driver.S
index 2d4d57d..0bf5f35 100644
--- a/bootloader/sp_driver.S
+++ b/bootloader/sp_driver.S
@@ -167,7 +167,7 @@ SP_ReadWord:
 ; ---
 
 .section .text
-.global SP_ReadCalibrationByte	
+.global SP_ReadCalibrationByte
 
 SP_ReadCalibrationByte:
 	ldi	r20, NVM_CMD_READ_CALIB_ROW_gc    ; Prepare NVM command in R20.
@@ -185,7 +185,7 @@ SP_ReadCalibrationByte:
 ;     R24 - Signature byte.
 ; ---
 
-.section .text	
+.section .text
 .global SP_ReadUserSignatureByte
 
 SP_ReadUserSignatureByte:
@@ -204,7 +204,7 @@ SP_ReadUserSignatureByte:
 ;     R24 - Fuse byte.
 ; ---
 
-.section .text	
+.section .text
 .global SP_ReadFuseByte
 
 SP_ReadFuseByte:
@@ -230,7 +230,7 @@ SP_ReadFuseByte:
 ;     Nothing.
 ; ---
 
-.section .text	
+.section .text
 .global SP_WriteLockBits
 
 SP_WriteLockBits:
@@ -250,7 +250,7 @@ SP_WriteLockBits:
 ;     R24 - Lock bits.
 ; ---
 
-.section .text		
+.section .text
 .global SP_ReadLockBits
 
 SP_ReadLockBits:
@@ -333,7 +333,7 @@ SP_EraseApplicationSection:
 ;     Nothing.
 ; ---
 
-.section .text	
+.section .text
 .global SP_EraseApplicationPage
 
 SP_EraseApplicationPage:
@@ -381,7 +381,7 @@ SP_LoadFlashWord:
 ; Returns:
 ;     Nothing.
 ; ---
-		
+
 ;.section .BOOT, "ax"
 .global SP_LoadFlashPage
 
@@ -436,7 +436,7 @@ SP_LoadFlashPage_1:
 ;     Nothing.
 ; ---
 
-.section .text		
+.section .text
 .global SP_ReadFlashPage
 
 SP_ReadFlashPage:
@@ -453,7 +453,7 @@ SP_ReadFlashPage:
 
 #if APP_SECTION_PAGE_SIZE > 512
 	ldi	r22, ((APP_SECTION_PAGE_SIZE/2) >> 8) ; Load R22 with byte cont high if flash page is large.
-#endif	
+#endif
 
 	ldi	r21, ((APP_SECTION_PAGE_SIZE)&0xFF)   ; Load R21 with byte count.
 
@@ -468,7 +468,7 @@ SP_ReadFlashPage_1:
 	sbci	r22, 0
 #else
 	dec	r21                             ; Decrement word count.
-#endif	
+#endif
 
 	brne	SP_ReadFlashPage_1              ; Repeat until byte count is zero.
 
@@ -488,7 +488,7 @@ SP_ReadFlashPage_1:
 ;     Nothing.
 ; ---
 
-.section .text		
+.section .text
 .global SP_WriteApplicationPage
 
 SP_WriteApplicationPage:
@@ -534,7 +534,7 @@ SP_EraseWriteApplicationPage:
 ;     Nothing.
 ; ---
 
-.section .text		
+.section .text
 .global SP_EraseFlashBuffer
 
 SP_EraseFlashBuffer:
@@ -555,7 +555,7 @@ SP_EraseFlashBuffer:
 ;     Nothing.
 ; ---
 
-.section .text		
+.section .text
 .global SP_EraseBootPage
 
 SP_EraseBootPage:
@@ -578,7 +578,7 @@ SP_EraseBootPage:
 ;     Nothing.
 ; ---
 
-.section .text		
+.section .text
 .global SP_WriteBootPage
 
 SP_WriteBootPage:
@@ -602,7 +602,7 @@ SP_WriteBootPage:
 ;     Nothing.
 ; ---
 
-.section .text		
+.section .text
 .global SP_EraseWriteBootPage
 
 SP_EraseWriteBootPage:
@@ -624,7 +624,7 @@ SP_EraseWriteBootPage:
 ;     R25:R24:R23:R22 - 32-bit CRC result (actually only 24-bit used).
 ; ---
 
-.section .text	
+.section .text
 .global SP_ApplicationCRC
 
 SP_ApplicationCRC:
@@ -671,7 +671,7 @@ SP_LockSPM:
 	ldi	r18, NVM_SPMLOCK_bm   ; Prepare bitmask for locking SPM into R18.
 	sts	NVM_CTRLB, r18        ; Load bitmask into NVM Control Register B, which locks SPM.
 	ret
-	
+
 
 
 ; ---
@@ -688,7 +688,7 @@ SP_LockSPM:
 ; ---
 
 .section .text
-.global SP_WaitForSPM		
+.global SP_WaitForSPM
 
 SP_WaitForSPM:
 	lds	r18, NVM_STATUS     ; Load the NVM Status register.
@@ -717,7 +717,7 @@ SP_WaitForSPM:
 ;     R25:R24:R23:R22 - 32-bit result from NVM operation.
 ; ---
 
-.section .text		
+.section .text
 
 SP_CommonCMD:
 	sts	NVM_CMD, r20        ; Load command into NVM Command register.
@@ -748,7 +748,7 @@ SP_CommonCMD:
 ;     R24     - Result from LPM operation.
 ; ---
 
-.section .text		
+.section .text
 
 SP_CommonLPM:
 	movw	ZL, r24             ; Load index into Z.
@@ -788,7 +788,6 @@ SP_CommonSPM:
 	clr	r1               ; Clear R1 for GCC _zero_reg_ to function properly.
 	out	RAMPZ, r19       ; Restore RAMPZ register.
 	ret
-	
-	
+
 ; END OF FILE
 
diff --git a/makefile b/makefile
index e86ce53..7b3e9c2 100644
--- a/makefile
+++ b/makefile
@@ -1,3 +1,6 @@
 SRC_USB += $(USB_PATH)/usb_xmega.c
 SRC_USB += $(USB_PATH)/usb_requests.c
 USB_OPTS += -I $(USB_PATH)
+
+# workaround for bad code generation on avr-gcc on linux (version 6.2.0)
+CFLAGS += -fno-jump-tables