Some initial bringup of the dfu bootloader on the mk20dx256vlh7

Bootloader/CMakeLists.txt

@@ -20,8 +20,8 @@
 #| You _MUST_ clean the build directory if you change this value
 #|
 set( CHIP
-	"mk20dx128vlf5"    # McHCK       mk20dx128vlf5
-#       "mk20dx256vlh7"    # Kiibohd-dfu mk20dx256vlh7
+	#"mk20dx128vlf5"    # McHCK       mk20dx128vlf5
+	"mk20dx256vlh7"    # Kiibohd-dfu mk20dx256vlh7
 )
 
 

Bootloader/Scripts/swdLoad.bash

@@ -21,11 +21,13 @@ fi
 
 # Make sure the toolchain is up to date
 cd programmer
-git pull --rebase
+#git pull --rebase
 cd ..
 
 # Attempt to flash
 # Udev rules have been applied to name the buspirate as /dev/buspirate (instead of something like /dev/ttyUSB0)
 # By default only root can access serial devices on Linux
+#ruby programmer/flash.rb name=buspirate:dev=/dev/buspirate --mass-erase
 ruby programmer/flash.rb name=buspirate:dev=/dev/buspirate "$1" "$2"
+#ruby programmer/flash.rb name=buspirate:dev=/dev/buspirate --mass-erase "$1" "$2"
 

Bootloader/_buildvars.h

@@ -32,7 +32,7 @@
 #define STR_MANUFACTURER        u"Kiibohd"
 #define STR_PRODUCT             u"Kiibohd DFU Bootloader"
 #define STR_SERIAL              u"@CHIP@"
-#define STR_ALTNAME             u"@BOOT_DFU_ALTNAME"
+#define STR_ALTNAME             u"@BOOT_DFU_ALTNAME@"
 
 
 // Mac OS-X and Linux automatically load the correct drivers.  On

Bootloader/dfu.c

@@ -1,5 +1,5 @@
 /* Copyright (c) 2011,2012 Simon Schubert <2@0x2c.org>.
- * Modifications by Jacob Alexander 2014 <haata@kiibohd.com>
+ * Modifications by Jacob Alexander 2014-2015 <haata@kiibohd.com>
  *
  * 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
@@ -98,12 +98,80 @@ static int dfu_handle_control( struct usb_ctrl_req_t *req, void *data )
 			dfu_dnload_complete(NULL, 0, ctx);
 		goto out_no_status;
 	}
+	case USB_CTRL_REQ_DFU_UPLOAD: {
+		return (0); // TODO
+		/*
+		void *buf;
+
+		switch (ctx->state) {
+		case DFU_STATE_dfuIDLE:
+			ctx->off = 0;
+			break;
+		case DFU_STATE_dfuUPLOAD_IDLE:
+			break;
+		default:
+			goto err;
+		}
+
+		// XXX Don't STALL? -HaaTa
+		// TODO
+		ctx->status = ctx->setup_write(ctx->off, req->wLength, &buf);
+		if (ctx->status != DFU_STATUS_OK) {
+			ctx->state = DFU_STATE_dfuERROR;
+			goto err_have_status;
+		}
+
+		if (req->wLength > 0)
+			usb_ep0_rx(buf, req->wLength, dfu_dnload_complete, ctx);
+		else
+			dfu_dnload_complete(NULL, 0, ctx);
+		goto out_no_status;
+		*/
+	}
 	case USB_CTRL_REQ_DFU_GETSTATUS: {
 		struct dfu_status_t st;
 
 		st.bState = ctx->state;
 		st.bStatus = ctx->status;
 		st.bwPollTimeout = 1000; /* XXX */
+
+		// XXX FAKE WRITE
+		if ( ctx->state == DFU_STATE_dfuMANIFEST )
+		{
+			uint8_t data[] = { 0x10, 0x20, 0x30, 0x40 };
+			flash_program_longword((uintptr_t)&_app_rom, data);
+		}
+		/*
+
+			uint32_t *position = &_app_rom + 0x100;
+		for ( ; position < &_app_rom + 0x200; position++ )
+		//for ( ; position < &_app_rom + 0x800; position++ )
+		{
+			if ( *position != 0xFFFFFFFF )
+			{
+			while( 1 )
+			{
+				GPIOA_PTOR |= (1<<5);
+				for (uint32_t d = 0; d < 7200000; d++ );
+			}
+			}
+		}*/
+
+		// Check to see if vector table was flashed correctly
+		// Return a flash error if it was not
+		if (_app_rom == 0xffffffff && ctx->state == DFU_STATE_dfuMANIFEST)
+			st.bStatus = DFU_STATUS_errPROG;
+		//}
+		/*
+		if (ctx->state == DFU_STATE_dfuMANIFEST)
+		{
+			uint8_t *addr = (uint8_t*)_app_rom;
+			//while (*(addr++) != 0x80);
+			//st.bStatus = DFU_STATUS_errPROG;
+			st.bStatus = (uint8_t)((uint32_t)(&_app_rom) >> 16);
+		}
+		*/
+
 		/**
 		 * If we're in DFU_STATE_dfuMANIFEST, we just finished
 		 * the download, and we're just about to send our last
@@ -131,14 +199,13 @@ static int dfu_handle_control( struct usb_ctrl_req_t *req, void *data )
 		switch (ctx->state) {
 		case DFU_STATE_dfuIDLE:
 		case DFU_STATE_dfuDNLOAD_IDLE:
-		/* case DFU_STATE_dfuUPLOAD_IDLE: */
+		case DFU_STATE_dfuUPLOAD_IDLE:
 			ctx->state = DFU_STATE_dfuIDLE;
 			break;
 		default:
 			goto err;
 		}
 		break;
-	/* case USB_CTRL_REQ_DFU_UPLOAD: */
 	default:
 		return (0);
 	}

Bootloader/dfu.desc.c

@@ -1,5 +1,5 @@
 // Originally Generated from MCHCK Toolkit
-/* Copyright (c) Jacob Alexander 2014 <haata@kiibohd.com>
+/* Copyright (c) Jacob Alexander 2014-2015 <haata@kiibohd.com>
  *
  * 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
@@ -61,7 +61,7 @@ static const struct usb_config_1 usb_config_1 = {
 		},
 		.will_detach = 1,
 		.manifestation_tolerant = 0,
-		.can_upload = 0,
+		.can_upload = 1,
 		.can_download = 1,
 		.wDetachTimeOut = 0,
 		.wTransferSize = USB_DFU_TRANSFER_SIZE,

Bootloader/flash.c

@@ -1,5 +1,5 @@
 /* Copyright (c) 2011,2012 Simon Schubert <2@0x2c.org>.
- * Modifications by Jacob Alexander 2014 <haata@kiibohd.com>
+ * Modifications by Jacob Alexander 2014-2015 <haata@kiibohd.com>
  *
  * 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
@@ -35,14 +35,18 @@ int ftfl_submit_cmd(void)
 {
 	FTFL.fstat.raw = ((struct FTFL_FSTAT_t){
 			.ccif = 1,
-				.rdcolerr = 1,
-				.accerr = 1,
-				.fpviol = 1
-				}).raw;
+			//.rdcolerr = 1,
+			.accerr = 1,
+			.fpviol = 1
+		}).raw;
+
+	// Wait for the operation to complete
 	struct FTFL_FSTAT_t stat;
-	while (!(stat = FTFL.fstat).ccif)
-		/* NOTHING */; /* XXX maybe WFI? */
-	return (!!stat.mgstat0);
+	while (!(stat = FTFL.fstat).ccif); // XXX maybe WFI?
+
+	// Mask error bits
+	return stat.raw & (FTFL_FSTAT_RDCOLERR | FTFL_FSTAT_ACCERR | FTFL_FSTAT_FPVIOL | FTFL_FSTAT_MGSTAT0);
+	//return (!!stat.mgstat0);
 }
 
 int flash_prepare_flashing(void)
@@ -66,20 +70,65 @@ int flash_erase_sector(uintptr_t addr)
 	return (ftfl_submit_cmd());
 }
 
-int flash_program_section(uintptr_t addr, size_t num_words)
+int flash_program_section_longwords(uintptr_t addr, size_t num_words)
 {
 	FTFL.fccob.program_section.fcmd = FTFL_FCMD_PROGRAM_SECTION;
 	FTFL.fccob.program_section.addr = addr;
 	FTFL.fccob.program_section.num_words = num_words;
-	return (ftfl_submit_cmd());
+
+	return ftfl_submit_cmd();
+}
+
+int flash_program_section_phrases(uintptr_t addr, size_t num_phrases)
+{
+	FTFL.fccob.program_section.fcmd = FTFL_FCMD_PROGRAM_SECTION;
+	FTFL.fccob.program_section.addr = addr;
+	FTFL.fccob.program_section.num_words = num_phrases;
+
+	return ftfl_submit_cmd();
+}
+
+int flash_program_longword(uintptr_t addr, uint8_t *data)
+{
+	FTFL.fccob.program_longword.fcmd = FTFL_FCMD_PROGRAM_LONGWORD;
+	FTFL.fccob.program_longword.addr = addr;
+	FTFL.fccob.program_longword.data_be[0] = data[0];
+	FTFL.fccob.program_longword.data_be[1] = data[1];
+	FTFL.fccob.program_longword.data_be[2] = data[2];
+	FTFL.fccob.program_longword.data_be[3] = data[3];
+
+	return ftfl_submit_cmd();
 }
 
 int flash_program_sector(uintptr_t addr, size_t len)
 {
+#if defined(_mk20dx128vlf5_)
 	return (len != FLASH_SECTOR_SIZE ||
 		(addr & (FLASH_SECTOR_SIZE - 1)) != 0 ||
 		flash_erase_sector(addr) ||
-		flash_program_section(addr, FLASH_SECTOR_SIZE/4));
+		flash_program_section_longwords(addr, FLASH_SECTOR_SIZE / 4));
+#elif defined(_mk20dx256vlh7_)
+	/*
+	return (len != FLASH_SECTOR_SIZE ||
+		(addr & (FLASH_SECTOR_SIZE - 1)) != 0 ||
+		flash_erase_sector(addr) ||
+		flash_program_section_phrases(addr, FLASH_SECTOR_SIZE / 8));
+	*/
+	return (len != FLASH_SECTOR_SIZE ||
+		(addr & (FLASH_SECTOR_SIZE - 1)) != 0 ||
+		flash_erase_sector(addr) ||
+		flash_program_section_phrases(addr, FLASH_SECTOR_SIZE / 8));
+#endif
+}
+
+int flash_prepare_reading(void)
+{
+	return (0);
+}
+
+int flash_read_sector(uintptr_t addr, size_t len)
+{
+	return (0);
 }
 
 void *flash_get_staging_area(uintptr_t addr, size_t len)

Bootloader/flash.h

@@ -1,5 +1,5 @@
 /* Copyright (c) 2011,2012 Simon Schubert <2@0x2c.org>.
- * Modifications by Jacob Alexander 2014 <haata@kiibohd.com>
+ * Modifications by Jacob Alexander 2014-2015 <haata@kiibohd.com>
  *
  * 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
@@ -20,7 +20,11 @@
 
 // ----- Defines -----
 
+#if defined(_mk20dx128vlf5_)
 #define FLASH_SECTOR_SIZE 1024
+#elif defined(_mk20dx256vlh7_)
+#define FLASH_SECTOR_SIZE 2048
+#endif
 
 
 
@@ -30,8 +34,9 @@ __attribute__((section(".ramtext.ftfl_submit_cmd"), long_call))
 int ftfl_submit_cmd(void);
 int flash_prepare_flashing(void);
 int flash_erase_sector(uintptr_t);
-int flash_program_section(uintptr_t, size_t);
+//int flash_program_section(uintptr_t, size_t);
 int flash_program_sector(uintptr_t, size_t);
+int flash_program_longword(uintptr_t, uint8_t*);
 void *flash_get_staging_area(uintptr_t, size_t);
 
 #endif

Bootloader/main.c

@@ -28,27 +28,29 @@
 /**
  * Unfortunately we can't DMA directly to FlexRAM, so we'll have to stage here.
  */
-static char staging[FLASH_SECTOR_SIZE];
+static char staging[ FLASH_SECTOR_SIZE ];
 
 
 
 // ----- Functions -----
 
-static enum dfu_status setup_write(size_t off, size_t len, void **buf)
+static enum dfu_status setup_write( size_t off, size_t len, void **buf )
 {
+	GPIOA_PCOR |= (1<<5);
 	static int last = 0;
 
-	if (len > sizeof(staging))
+	if ( len > sizeof(staging) )
 		return (DFU_STATUS_errADDRESS);
 
 	// We only allow the last write to be less than one sector size.
-	if (off == 0)
+	if ( off == 0 )
 		last = 0;
-	if (last && len != 0)
+	if ( last && len != 0 )
 		return (DFU_STATUS_errADDRESS);
-	if (len != FLASH_SECTOR_SIZE) {
+	if ( len != FLASH_SECTOR_SIZE )
+	{
 		last = 1;
-		memset(staging, 0xff, sizeof(staging));
+		memset( staging, 0xff, sizeof(staging) );
 	}
 
 	*buf = staging;
@@ -58,16 +60,30 @@ static enum dfu_status setup_write(size_t off, size_t len, void **buf)
 static enum dfu_status finish_write( void *buf, size_t off, size_t len )
 {
 	void *target;
-	if (len == 0)
+	if ( len == 0 )
 		return (DFU_STATUS_OK);
 
 	target = flash_get_staging_area(off + (uintptr_t)&_app_rom, FLASH_SECTOR_SIZE);
-	if (!target)
+	if ( !target )
 		return (DFU_STATUS_errADDRESS);
-	memcpy(target, buf, len);
-	if (flash_program_sector(off + (uintptr_t)&_app_rom, FLASH_SECTOR_SIZE) != 0)
+	memcpy( target, buf, len );
+
+	// Depending on the error return a different status
+	switch ( flash_program_sector(off + (uintptr_t)&_app_rom, FLASH_SECTOR_SIZE) )
+	{
+	/*
+	case FTFL_FSTAT_RDCOLERR: // Flash Read Collision Error
+	case FTFL_FSTAT_ACCERR:   // Flash Access Error
+	case FTFL_FSTAT_FPVIOL:   // Flash Protection Violation Error
 		return (DFU_STATUS_errADDRESS);
-	return (DFU_STATUS_OK);
+	case FTFL_FSTAT_MGSTAT0:  // Memory Controller Command Completion Error
+		return (DFU_STATUS_errADDRESS);
+	*/
+
+	case 0:
+	default: // No error
+		return (DFU_STATUS_OK);
+	}
 }
 
 
@@ -93,17 +109,51 @@ void main()
 	// Enabling LED to indicate we are in the bootloader
 	GPIOA_PDDR |= (1<<5);
 	// Setup pin - A5 - See Lib/pin_map.mchck for more details on pins
-	PORTA_PCR19 = PORT_PCR_SRE | PORT_PCR_DSE | PORT_PCR_MUX(1);
+	PORTA_PCR5 = PORT_PCR_SRE | PORT_PCR_DSE | PORT_PCR_MUX(1);
 	GPIOA_PSOR |= (1<<5);
-
+#else
+#error "Incompatible chip for bootloader"
 #endif
 
+	//for (uint8_t c = 0; c < 20; c++)
+	/*
+	while( 1 )
+	{
+		GPIOA_PTOR |= (1<<5);
+		for (uint32_t d = 0; d < 7200000; d++ );
+	}
+	*/
+
+	// XXX REMOVEME
+	/*
+	GPIOB_PDDR |= (1<<16);
+	PORTB_PCR16 = PORT_PCR_SRE | PORT_PCR_DSE | PORT_PCR_MUX(1);
+	GPIOB_PSOR |= (1<<16);
+	*/
+
 	flash_prepare_flashing();
 
+	uint32_t *position = &_app_rom;
 	usb_init( &dfu_device );
 	for (;;)
 	{
 		usb_poll();
+
+		/*
+		for ( ; position < &_app_rom + 0x201; position++ )
+		//for ( ; position < &_app_rom + 0x800; position++ )
+		{
+			if ( *position != 0xFFFFFFFF )
+			{
+			while( 1 )
+			{
+				GPIOA_PTOR |= (1<<5);
+				for (uint32_t d = 0; d < 7200000; d++ );
+			}
+			}
+		}
+		*/
+
 	}
 }
 

Debug/cli/cli.h

@@ -55,7 +55,7 @@
 	const PROGMEM char name##CLIDict_DescEntry[] = description;
 
 // ARM is easy :P
-#elif defined(_mk20dx128_) || defined(_mk20dx128vlf5_) || defined(_mk20dx256_) // ARM
+#elif defined(_mk20dx128_) || defined(_mk20dx128vlf5_) || defined(_mk20dx256_) || defined(_mk20dx256vlh7_) // ARM
 #define CLIDict_Def(name,description) \
 	const char name##Name[] = description; \
 	const CLIDictItem name[]

Lib/mk20dx.c

@@ -394,7 +394,7 @@ const uint8_t flashconfigbytes[16] = {
 	//  http://cache.freescale.com/files/microcontrollers/doc/app_note/AN4507.pdf
 	//  http://cache.freescale.com/files/32bit/doc/ref_manual/K20P64M72SF1RM.pdf (28.34.6)
 	//
-	0xFF, 0xFF, 0xFF, 0xFE, // Program Flash Protection Bytes FPROT0-3
+	0xFF, 0xFF, 0xFF, 0xFF, // Program Flash Protection Bytes FPROT0-3 // XXX TODO PROTECT
 
 	0xBE, // Flash security byte FSEC
 	0x03, // Flash nonvolatile option byte FOPT
@@ -469,8 +469,17 @@ void ResetHandler()
 	//
 	// Also checking for ARM lock-up signal (invalid firmware image)
 	// RCM_SRS1 & 0x02
-	if ( RCM_SRS0 & 0x40 || RCM_SRS0 & 0x20 || RCM_SRS1 & 0x02 || _app_rom == 0xffffffff ||
-	  memcmp( (uint8_t*)&VBAT, sys_reset_to_loader_magic, sizeof(sys_reset_to_loader_magic) ) == 0 ) // Check for soft reload
+	if (    // PIN  (External Reset Pin/Switch)
+		   RCM_SRS0 & 0x40
+		// WDOG (Watchdog timeout)
+		|| RCM_SRS0 & 0x20
+		// LOCKUP (ARM Core LOCKUP event)
+		|| RCM_SRS1 & 0x02
+		// Blank flash check
+		|| _app_rom == 0xffffffff
+		// Software reset
+		|| memcmp( (uint8_t*)&VBAT, sys_reset_to_loader_magic, sizeof(sys_reset_to_loader_magic) ) == 0
+	)
 	{
 		memset( (uint8_t*)&VBAT, 0, sizeof(VBAT) );
 	}

Lib/mk20dx128.ld

@@ -85,10 +85,10 @@ SECTIONS
 
 	.data : AT (_etext) {
 		. = ALIGN(4);
-		_sdata = .; 
+		_sdata = .;
 		*(.data*)
 		. = ALIGN(4);
-		_edata = .; 
+		_edata = .;
 	} > RAM
 
 	.noinit (NOLOAD) : {
@@ -97,7 +97,7 @@ SECTIONS
 
 	.bss : {
 		. = ALIGN(4);
-		_sbss = .; 
+		_sbss = .;
 		*(.bss*)
 		*(COMMON)
 		. = ALIGN(4);

Lib/mk20dx128vlf5.bootloader.ld

@@ -50,12 +50,10 @@ SECTIONS
 {
 	.text :
 	{
-		. = 0;
-		KEEP(* (.vectors))
+		. = 0; KEEP(* (.vectors)) /* MUST BE AT 0 */
 		*(.startup*)
 		*(.rodata*)
-		. = 0x400;
-		KEEP(* (.flashconfig))
+		. = 0x400; KEEP(* (.flashconfig)) /* MUST BE AT 0x400 */
 		*(.text*)
 		. = ALIGN(4);
 		KEEP(*(.init))

Lib/mk20dx256.ld

@@ -10,10 +10,10 @@
  * permit persons to whom the Software is furnished to do so, subject to
  * the following conditions:
  *
- * 1. The above copyright notice and this permission notice shall be 
+ * 1. The above copyright notice and this permission notice shall be
  * included in all copies or substantial portions of the Software.
  *
- * 2. If the Software is incorporated into a build system that allows 
+ * 2. If the Software is incorporated into a build system that allows
  * selection among a list of target devices, then similar target
  * devices manufactured by PJRC.COM must be included in the list of
  * target devices and selectable in the same manner.
@@ -50,10 +50,10 @@ MEMORY
  * permit persons to whom the Software is furnished to do so, subject to
  * the following conditions:
  *
- * 1. The above copyright notice and this permission notice shall be 
+ * 1. The above copyright notice and this permission notice shall be
  * included in all copies or substantial portions of the Software.
  *
- * 2. If the Software is incorporated into a build system that allows 
+ * 2. If the Software is incorporated into a build system that allows
  * selection among a list of target devices, then similar target
  * devices manufactured by PJRC.COM must be included in the list of
  * target devices and selectable in the same manner.
@@ -118,10 +118,10 @@ SECTIONS
 
 	.data : AT (_etext) {
 		. = ALIGN(4);
-		_sdata = .; 
+		_sdata = .;
 		*(.data*)
 		. = ALIGN(4);
-		_edata = .; 
+		_edata = .;
 	} > RAM
 
 	.noinit (NOLOAD) : {
@@ -130,7 +130,7 @@ SECTIONS
 
 	.bss : {
 		. = ALIGN(4);
-		_sbss = .; 
+		_sbss = .;
 		*(.bss*)
 		*(COMMON)
 		. = ALIGN(4);

Lib/mk20dx256vlh7.bootloader.ld

@@ -1,7 +1,7 @@
 /* Teensyduino Core Library
  * http://www.pjrc.com/teensy/
  * Copyright (c) 2013 PJRC.COM, LLC.
- * Modifications by Jacob Alexander 2014 for use with McHCK and Kiibohd-dfu
+ * Modifications by Jacob Alexander 2014-2015 for use with McHCK and Kiibohd-dfu
  *
  * Permission is hereby granted, free of charge, to any person obtaining
  * a copy of this software and associated documentation files (the
@@ -29,8 +29,6 @@
  * SOFTWARE.
  */
 
-/* XXX Not tested yet -HaaTa */
-
 MEMORY
 {
 	FLASH (rx)     : ORIGIN = 0x0, LENGTH = 256K
@@ -52,12 +50,10 @@ SECTIONS
 {
 	.text :
 	{
-		. = 0;
-		KEEP(* (.vectors))
+		. = 0; KEEP(* (.vectors)) /* MUST BE AT 0 */
 		*(.startup*)
-		. = 0x400;
+		. = 0x400; KEEP(* (.flashconfig)) /* MUST BE AT 0x400 */
 		*(.rodata*)
-		KEEP(* (.flashconfig))
 		*(.text*)
 		. = ALIGN(4);
 		KEEP(*(.init))

Lib/mk20dx256vlh7.ld

@@ -1,7 +1,7 @@
 /* Teensyduino Core Library
  * http://www.pjrc.com/teensy/
  * Copyright (c) 2013 PJRC.COM, LLC.
- * Modifications by Jacob Alexander 2014 for use with McHCK and Kiibohd-dfu
+ * Modifications by Jacob Alexander 2014-2015 for use with McHCK and Kiibohd-dfu
  *
  * Permission is hereby granted, free of charge, to any person obtaining
  * a copy of this software and associated documentation files (the
@@ -29,8 +29,6 @@
  * SOFTWARE.
  */
 
-/* XXX Not tested yet -HaaTa */
-
 MEMORY
 {
 	FLASH (rx) : ORIGIN = 8K, LENGTH = 256K-8K

Scan/ISSILed/led_scan.c

@@ -271,7 +271,6 @@ inline void I2C_setup()
 	I2C0_FLT = 4;
 	I2C0_C1 = I2C_C1_IICEN;
 	I2C0_C2 = I2C_C2_HDRS; // High drive select
-//},
 
 	// Enable I2C Interrupt
 	NVIC_ENABLE_IRQ( IRQ_I2C0 );

main.c

@@ -39,6 +39,17 @@
 
 int main()
 {
+	/*
+	GPIOA_PDDR |= (1<<5);
+	// Setup pin - A5 - See Lib/pin_map.mchck for more details on pins
+	PORTA_PCR5 = PORT_PCR_SRE | PORT_PCR_DSE | PORT_PCR_MUX(1);
+	while( 1 )
+	{
+		GPIOA_PTOR |= (1<<5);
+		for (uint32_t d = 0; d < 720000; d++ );
+	}
+	*/
+
 	// AVR - Teensy Set Clock speed to 16 MHz
 #if defined(_at90usb162_) || defined(_atmega32u4_) || defined(_at90usb646_) || defined(_at90usb1286_)
 	CLKPR = 0x80;