From b9fa9c8672a4d1fd89c0c11a358536dfe3ec28a1 Mon Sep 17 00:00:00 2001
From: Jacob Alexander <haata@kiibohd.com>
Date: Mon, 25 May 2015 17:50:32 -0700
Subject: [PATCH] Fixing bugs in mk20dx128vlf5 support

---
 Bootloader/CMakeLists.txt |  9 ++++++++-
 Bootloader/flash.c        | 17 +++++++++++------
 Bootloader/main.c         |  7 +++----
 3 files changed, 22 insertions(+), 11 deletions(-)

diff --git a/Bootloader/CMakeLists.txt b/Bootloader/CMakeLists.txt
index 7a938d9..41c2bcc 100644
--- a/Bootloader/CMakeLists.txt
+++ b/Bootloader/CMakeLists.txt
@@ -60,7 +60,6 @@ include( initialize )
 #
 set( SRCS
 	main.c
-	debug.c # TODO only compile in if necessary
 	dfu.c
 	dfu.desc.c
 	flash.c
@@ -68,6 +67,14 @@ set( SRCS
 	usb.c
 )
 
+# Only compile in if necessary
+if( CHIP STREQUAL "mk20dx256vlh7" )
+	set( SRCS ${SRCS}
+		debug.c
+	)
+endif()
+
+
 message( STATUS "Bootloader Source Files:" )
 message( "${SRCS}" )
 
diff --git a/Bootloader/flash.c b/Bootloader/flash.c
index 349a44e..d04cf38 100644
--- a/Bootloader/flash.c
+++ b/Bootloader/flash.c
@@ -101,15 +101,20 @@ int flash_program_section_phrases( uintptr_t addr, size_t num_phrases )
 
 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_longwords( addr, FLASH_SECTOR_SIZE / 4 ));
-#elif defined(_mk20dx256vlh7_)
 	if ( len != FLASH_SECTOR_SIZE )
 		return 1;
 
+#if defined(_mk20dx128vlf5_)
+	// Check if this is the beginning of a sector
+	// Only erase if necessary
+	if ( (addr & (FLASH_SECTOR_SIZE - 1)) == 0
+		&& flash_read_1s_sector( addr, FLASH_SECTOR_SIZE / 4 )
+		&& flash_erase_sector( addr ) )
+			return 1;
+
+	// Program sector
+	return flash_program_section_longwords( addr, FLASH_SECTOR_SIZE / 4 );
+#elif defined(_mk20dx256vlh7_)
 	// Check if beginning of sector and erase if not empty
 	// Each sector is 2 kB in length, but we can only write to half a sector at a time
 	// We can only erase an entire sector at a time
diff --git a/Bootloader/main.c b/Bootloader/main.c
index 303ea35..8ca8d28 100644
--- a/Bootloader/main.c
+++ b/Bootloader/main.c
@@ -36,7 +36,7 @@ static char staging[ USB_DFU_TRANSFER_SIZE ];
 
 // ----- Functions -----
 
-void sector_print( void* buf, size_t sector, size_t chunks )
+int sector_print( void* buf, size_t sector, size_t chunks )
 {
 	uint8_t* start = (uint8_t*)buf + sector * USB_DFU_TRANSFER_SIZE;
 	uint8_t* end = (uint8_t*)buf + (sector + 1) * USB_DFU_TRANSFER_SIZE;
@@ -79,6 +79,8 @@ void sector_print( void* buf, size_t sector, size_t chunks )
 
 		print( NL );
 	}
+
+	return retval;
 }
 
 static enum dfu_status setup_read( size_t off, size_t *len, void **buf )
@@ -133,9 +135,6 @@ static enum dfu_status finish_write( void *buf, size_t off, size_t len )
 	if ( !target )
 		return (DFU_STATUS_errADDRESS);
 	memcpy( target, buf, len );
-	print("BUF: ");
-	printHex( off );
-	sector_print( target, 0, 16 );
 
 	// Depending on the error return a different status
 	switch ( flash_program_sector(off + (uintptr_t)&_app_rom, FLASH_SECTOR_SIZE) )