Index: firmware/App/Services/FPGA.c
===================================================================
diff -u -r567a9736641b877ac0028db7481daad2dc65305f -re8511af1e4e9d91cfd3378471869fb0246110870
--- firmware/App/Services/FPGA.c	(.../FPGA.c)	(revision 567a9736641b877ac0028db7481daad2dc65305f)
+++ firmware/App/Services/FPGA.c	(.../FPGA.c)	(revision e8511af1e4e9d91cfd3378471869fb0246110870)
@@ -14,6 +14,7 @@
 #include "Download.h"
 #include "FPGA.h"
 #include "Timers.h"
+#include "SystemComm.h"
 #include "Utilities.h"
 
 #define QUEUE_MAX_SIZE                  20                                                                ///< Max queue size.
@@ -155,8 +156,8 @@
 
 static U32 TESTREMOVE = 0; // TODO remove
 static U32 countRemove = 0; // TODO remove
-static U32 nonzeroCounter = 0; // TODO remove
-static U16 nonZeroCount[10000]; // TODO remove
+static U16 tempFlashStatus;
+static U08 tempACkStatus = 0;
 
 static const U08 STACK_FPGA_ID[ NUM_OF_FW_STACKS ] = { 0x5A, 0x61, 0xFF };  // TODO update with the real FPGA IDs
 static const U16 DISABLE_UPDATE_REG_CMD            = 5; // TODO what is this value? 0?
@@ -194,7 +195,7 @@
 static void resetFPGACommFlags( void );
 static void enqueue( FPGA_JOBS_T job );
 static void dequeue( void );
-static U08 peekFromQueue( void );
+static FPGA_JOBS_T peekFromQueue( void );
 static BOOL isQueueFull( void );
 
 static FPGA_STATE_T handleFPGAIdleState( void );
@@ -216,14 +217,31 @@
 
     enqueue( FPGA_READ_HEADER );
     enqueue( FPGA_READ_UPDATE_REG );
-    enqueue( FPGA_READ_MULTI_BOOT_STATUS );
 
     fpgaState                = FPGA_IDLE_STATE;
     fpgaUpdateRegisterStatus = 0;
 }
 
 void execFPGA( void )
 {
+    if ( TRUE == fpgaFlashStatus.hasUpdateRegsBeenRqstd )
+    {
+        processFPGAFlashRegistersRead();
+    }
+
+    // TODO test code remove
+    U08 data[8];
+    data[0] = getTempRemoveMSGID();
+    data[1] = (U08)fpgaState;
+    data[2] = GET_LSB_OF_WORD( fpgaFlashStatus.fifoRemainingCount );
+    data[3] = GET_MSB_OF_WORD( fpgaFlashStatus.fifoRemainingCount );
+    data[4] = GET_LSB_OF_WORD( tempFlashStatus );
+    data[5] = GET_MSB_OF_WORD( tempFlashStatus );
+    data[6] = tempACkStatus;
+    data[7] = 0;
+    broadcastDataTestRemove(data);
+    // TODO test code remove
+
     switch( fpgaState )
     {
         case FPGA_IDLE_STATE:
@@ -262,11 +280,6 @@
    else if ( FPGA_COMM_READ_IN_PROGRESS == fpgaJobsQStatus.fpgaCommRead )
    {
        fpgaJobsQStatus.fpgaCommRead = FPGA_COMM_READ_RESP_RECEIVED;
-
-       if ( TRUE == fpgaFlashStatus.hasUpdateRegsBeenRqstd )
-       {
-           processFPGAFlashRegistersRead();
-       }
    }
 }
 
@@ -312,11 +325,11 @@
     {
         memset( fpgaDataToWriteBuffer, 0x0, SW_UPDATE_FLASH_BUFFER_SIZE );
 
-        enqueue( FPGA_FLASH_WRITE_DATA );
-
         fpgaDataLenToWrite = len;
         memcpy( fpgaDataToWriteBuffer, data, len );
 
+        enqueue( FPGA_FLASH_WRITE_DATA );
+
         fpgaFlashStatus.fpgaFlashState = FPGA_UPDATE_BUSY;
     }
 }
@@ -473,6 +486,8 @@
             U16 fifoStatus  = MAKE_WORD_OF_BYTES( fpgaReadResponseBuffer[ FPGA_FLASH_STATUS_INDEX + sizeof( U16 ) ],
                                                   fpgaReadResponseBuffer[ FPGA_FLASH_STATUS_INDEX + sizeof( U16 ) - 1 ] );
 
+            tempFlashStatus = flashStatus; // TODO remove
+
             fpgaFlashStatus.fpgaFlashState         = FPGA_UPDATE_NOT_READY;
             fpgaFlashStatus.fifoRemainingCount     = FPGA_FIFO_SIZE_BYTES - ( FPGA_FIFO_COUNT_MASK & fifoStatus );
             fpgaFlashStatus.hasUpdateRegsBeenRqstd = FALSE;
@@ -496,15 +511,14 @@
                 fpgaFlashStatus.fpgaFlashState = FPGA_UPDATE_FIFO_FULL;
             }
 
-            nonZeroCount[nonzeroCounter] = fpgaFlashStatus.fifoRemainingCount;
-            nonzeroCounter = INC_WRAP( nonzeroCounter, 0, 10000 - 1 );
-
             // TODO remove
+            /*
             if ( fpgaFlashStatus.fifoRemainingCount < FPGA_FIFO_SIZE_BYTES )
             {
                 BOOL dara = FALSE;
             }
             // TODO remove
+             */
         }
     }
 }
@@ -646,14 +660,14 @@
     _enable_IRQ();
 }
 
-static U08 peekFromQueue( void )
+static FPGA_JOBS_T peekFromQueue( void )
 {
     _disable_IRQ();
-    U08 frontIndex       = fpgaJobsQStatus.fpgaJobFrontIndex;
-    U08 frontIndexUpdate = INC_WRAP( frontIndex, 0, QUEUE_MAX_SIZE - 1 );
+    U08 frontIndex      = fpgaJobsQStatus.fpgaJobFrontIndex;
+    FPGA_JOBS_T nextJob = (FPGA_JOBS_T)fpgaJobsQStatus.fpgaJobsQueue[ frontIndex ];
     _enable_IRQ();
 
-    return frontIndexUpdate;
+    return nextJob;
 }
 
 static BOOL isQueueFull( void )
@@ -672,8 +686,6 @@
 {
     FPGA_STATE_T state = FPGA_IDLE_STATE;
 
-    requestFlashRegistersStatus();
-
     if ( fpgaJobsQStatus.fpgaJobsQueueCount > 0 )
     {
         BOOL isDequeueAllowed = TRUE;
@@ -691,13 +703,10 @@
         }
     }
 
-    // TODo remove
-    if ( ( FPGA_FLASH_WRITE_DATA == fpgaJobsQStatus.fpgaCurrentJob ) && ( fpgaFlashStatus.fifoRemainingCount < SW_UPDATE_FLASH_BUFFER_SIZE ) )
-    {
-        nonZeroCount[ nonzeroCounter ] = fpgaFlashStatus.fifoRemainingCount;
-        nonzeroCounter++;
-    }
+    requestFlashRegistersStatus();
 
+    // TODo remove
+    tempACkStatus = NACK;
     if ( FPGA_SELF_CONFIGURE == fpgaJobsQStatus.fpgaCurrentJob )
     {
         BOOL test = FALSE;
@@ -775,6 +784,7 @@
                 {
                     fpgaFlashStatus.fpgaFlashState = FPGA_UPDATE_READY;
                     sendFPGAAckNackStatus( ACK );
+                    tempACkStatus = ACK;
 
                     TESTREMOVE += fpgaDataLenToWrite;// TODO REMOVE
                     countRemove += 1;// TODO REMOVE