Index: firmware/App/Services/NVDataMgmt.c
===================================================================
diff -u -rda12d1065b9bc93d30500255d8b986f00d1bdd69 -r9af6fc3e5afc442a877bd5e23ecfa6872a3ad5a4
--- firmware/App/Services/NVDataMgmt.c	(.../NVDataMgmt.c)	(revision da12d1065b9bc93d30500255d8b986f00d1bdd69)
+++ firmware/App/Services/NVDataMgmt.c	(.../NVDataMgmt.c)	(revision 9af6fc3e5afc442a877bd5e23ecfa6872a3ad5a4)
@@ -11,15 +11,18 @@
 
 #include "NVDataMgmt.h"
 
+/*
+ * TODO
+ * 1. Clean up the flash with the bad data packets
+ * 2. Reset the counter to start the write from 0x10000
+ */
+
 #define BANK0_NUM_OF_SECTORS            16
 #define FLOAT_TO_INT_ROUNDUP_OFFSET     0.5F        ///< Offset to add to a floating point value for rounding rounding when converting to integer
 
 #define ROUNDED_HCLK_FREQ               ((HCLK_FREQ) < 0.0F ? (S32)((HCLK_FREQ) - FLOAT_TO_INT_ROUNDUP_OFFSET) : \
                                                               (S32)((HCLK_FREQ) + FLOAT_TO_INT_ROUNDUP_OFFSET))               ///< Rounded HCLK for flash clock.
-#define FIRMWARE_START_ADDRESS          0x00010000
 
-#define MAX_FALSH_WRITE_BUFFER_BYTES        16U          // TODO remove                                                       ///< Max allowed bytes for an EEPROM write (16 bytes).
-
 /// EEPROM functions use the buffer length as the size of U32. So before send the length to any of FAPI functions, it should be divided by 4.
 #define EEPROM_OPS_SIZE_OF_CONVERTER         4
 
@@ -70,12 +73,16 @@
 {
     Fapi_initializeFlashBanks( ROUNDED_HCLK_FREQ );
     Fapi_setActiveFlashBank( Fapi_FlashBank0 );
-    Fapi_enableMainBankSectors(0xFFFF);                    /* used for API 2.01*/
+    Fapi_enableMainBankSectors( 0xFFFF );                    /* used for API 2.01*/
     while( FAPI_CHECK_FSM_READY_BUSY != Fapi_Status_FsmReady );
 
+    clearSWUpdateNVStatus();
+}
+
+void clearSWUpdateNVStatus( void )
+{
     SWUpdateFlashStatus.hasFlashBeenErased  = FALSE;
     SWUpdateFlashStatus.currentWriteAddress = FIRMWARE_START_ADDRESS;
-
 }
 
 BOOL handleUpdatingFlash( U08* dataToWrite )
@@ -84,16 +91,17 @@
 
     if ( FALSE == SWUpdateFlashStatus.hasFlashBeenErased )
     {
-        BOOL eraseStatus = FALSE;
+        // TODO what should we do if the erase failed? try again?
+        SWUpdateFlashStatus.hasFlashBeenErased = eraseFlashSectors();
 
-        eraseStatus                            = eraseFlashSectors();
-        SWUpdateFlashStatus.hasFlashBeenErased = ( TRUE == eraseStatus ? TRUE : FALSE );
-
-        writeFlashSectors( dataToWrite );
+        if ( TRUE == SWUpdateFlashStatus.hasFlashBeenErased )
+        {
+            status = writeFlashSectors( dataToWrite );
+        }
     }
     else
     {
-        writeFlashSectors( dataToWrite );
+        status = writeFlashSectors( dataToWrite );
     }
 
     return status;
@@ -154,7 +162,7 @@
 
    writeVerifyStatus = Fapi_doVerify( (U32*)startAddress, ( SW_UPDATE_FLASH_BUFFER_SIZE / SW_UPDATE_FLASH_BUFFER_SIZE ),
                                          (U32*)&dataRead2Verify, &writeVerifyReason );
-   while( FAPI_CHECK_FSM_READY_BUSY == Fapi_Status_FsmBusy );
+   while( FAPI_CHECK_FSM_READY_BUSY == Fapi_Status_FsmBusy ); // TODO timeout or count so we wont get stuck here for ever
 
    status = ( Fapi_Status_Success == writeVerifyStatus ? TRUE : FALSE );