Index: firmware/App/Services/FPGA.c
===================================================================
diff -u -rba60692a9ecaf59cb5cb8490f4276917f43bcd01 -r3a0dc476f0f42bb7ebadc7d6109a0b5b6581cce4
--- firmware/App/Services/FPGA.c	(.../FPGA.c)	(revision ba60692a9ecaf59cb5cb8490f4276917f43bcd01)
+++ firmware/App/Services/FPGA.c	(.../FPGA.c)	(revision 3a0dc476f0f42bb7ebadc7d6109a0b5b6581cce4)
@@ -40,6 +40,7 @@
 #define FPGA_FLASH_STATUS_REG_ADDR      0x0900                  ///< FPGA flash status register address.
 #define FPGA_FIFO_COUNT_REG_ADDR        0x0902                  ///< FPGA FIFO count register address.
 #define FPGA_FLASH_DATA_REG_ADDR        0x0A00                  ///< FPGA flash data register address.
+#define FPGA_ICAP2_REG_ADDR             0x0909                  ///< FPGA ICAP 2 command register address.
 
 #define FPGA_UPDATE_REGISTER_ADDR       ( FPGA_WRITE_START_ADDR + 4 )
 #define FPGA_READ_CMD_HDR_LEN           4                       ///< FPGA read command header byte length.
@@ -48,6 +49,9 @@
 
 #define UPDATE_REQUESTED_VALUE          1
 
+#define FPGA_ERASE_FIFO_CMD_OK          ( 1 << 11 )
+#define FPGA_FLASH_STATUS_OK            ( 1 << 15 )
+
 typedef enum
 {
     FPGA_COMM_IDLE = 0,
@@ -66,9 +70,11 @@
     FPGA_RESET_FLASH,
     FPGA_ERASE_FIFO,
     FPGA_ENABLE_FLASH,
-    FPGA_CHECK_FLASH_STATUS,
+    FPGA_CHECK_ERASE_FIFO_STATUS,
+    FPGA_CHECK_FLASH_READY_STATUS,
     FPGA_CHECK_FIFO_COUNT,
-    FPGA_FLASH_UPDATE_DATA,
+    FPGA_FLASH_WRITE_DATA,
+    FPGA_SELF_CONFIGURE,
     NUM_OF_FPGA_JOBS,
 } FPGA_JOBS_T;
 
@@ -82,6 +88,15 @@
     NUM_OF_FPGA_STATES
 } FPGA_STATE_T;
 
+typedef enum
+{
+    FPGA_UPDATE_NOT_STARTED = 0,
+    FPGA_UPDATE_INITIALIZE_FLASH,
+    FPGA_UPDATE_FLASH_IN_PROG,
+    FPGA_UPDATE_FINISH_FLASH,
+    NUM_OF_FPGA_FLASH_STATE,
+} FPGA_FLASH_STATE_T; // TODO remove?
+
 #pragma pack(push,1)
 typedef struct
 {
@@ -102,6 +117,14 @@
     FPGA_COMM_STATE_T fpgaCommWrite;
 } FPGA_JOBS_Q_STATUS_T;
 
+typedef struct
+{
+    BOOL isFlashErased;
+    BOOL isFlashReady;
+    U16 FIFOCount;
+    U32 startTime;
+} FPGA_FLASH_STATUS_T;
+
 /// Record structure for FPGA header read.
 typedef struct
 {
@@ -135,20 +158,25 @@
 static const U08 FPGA_RESET_FLASH_CMD              = 0x01;
 static const U08 FPGA_ERASE_FIFO_CMD               = 0x08;
 static const U08 FPGA_ENABLE_FLASH_CMD             = 0x00;
+static const U08 FPGA_SELF_CONFIG_CMD              = 0x03;
+
 static const FPGA_JOB_SPECS_T JOBS_SPECS[ NUM_OF_FPGA_JOBS ] = {
   { FPGA_HEADER_START_ADDR,         sizeof( FPGA_HEADER_T ),       0,                              FALSE },        // FPGA_READ_HEADER
   { FPGA_BULK_READ_START_ADDR,      FPGA_MAX_READ_SIZE,            0,                              FALSE },        // FPGA_READ_UPDATE_REG
   { FPGA_UPDATE_REGISTER_ADDR,      sizeof( U16 ),                 (U08*)&DISABLE_UPDATE_REG_CMD,  TRUE  },        // FPGA_WRITE_UPDATE_REG
   { FPGA_FLASH_CONTROL_REG_ADDR,    sizeof( U08 ),                 (U08*)&FPGA_RESET_FLASH_CMD,    TRUE  },        // FPGA_RESET_FLASH
   { FPGA_FLASH_CONTROL_REG_ADDR,    sizeof( U08 ),                 (U08*)&FPGA_ERASE_FIFO_CMD,     TRUE  },        // FPGA_ERASE_FIFO
   { FPGA_FLASH_CONTROL_REG_ADDR,    sizeof( U08 ),                 (U08*)&FPGA_ENABLE_FLASH_CMD,   TRUE  },        // FPGA_ENABLE_FLASH
-  { FPGA_FLASH_STATUS_REG_ADDR,     sizeof( U16 ),                 0,                              FALSE },        // FPGA_CHECK_FLASH_STATUS
+  { FPGA_FLASH_STATUS_REG_ADDR,     sizeof( U16 ),                 0,                              FALSE },        // FPGA_CHECK_ERASE_FIFO_STATUS
+  { FPGA_FLASH_STATUS_REG_ADDR,     sizeof( U16 ),                 0,                              FALSE },        // FPGA_CHECK_FLASH_READY_STATUS
   { FPGA_FIFO_COUNT_REG_ADDR,       sizeof( U16 ),                 0,                              FALSE },        // FPGA_CHECK_FIFO_COUNT
-  { FPGA_FLASH_DATA_REG_ADDR,       SW_UPDATE_FLASH_BUFFER_SIZE,   fpgaWriteCmdBuffer,             TRUE  }         // FPGA_FLASH_UPDATE_DATA
+  { FPGA_FLASH_DATA_REG_ADDR,       SW_UPDATE_FLASH_BUFFER_SIZE,   fpgaWriteCmdBuffer,             TRUE  },        // FPGA_FLASH_UPDATE_DATA
+  { FPGA_ICAP2_REG_ADDR,            sizeof( U08 ),                 (U08*)&FPGA_SELF_CONFIG_CMD,    TRUE  }         // FPGA_SELF_CONFIGURE
 };
 
 static void initDMA( void );
 static void consumeUnexpectedData( void );
+static void processFPGAReceivedData( void );
 
 static void setupDMAForReadResp( U32 bytes2Receive );
 static void setupDMAForReadCmd( U32 bytes2Transmit );
@@ -234,9 +262,11 @@
 {
     BOOL status = FALSE;
 
-    if ( FPGA_IDLE_STATE == fpgaState )
+    if ( ( FPGA_IDLE_STATE == fpgaState ) && ( UPDATE_REQUESTED_VALUE == fpgaUpdateRegisterStatus ) )
     {
-        status = ( UPDATE_REQUESTED_VALUE == fpgaUpdateRegisterStatus ? TRUE : FALSE );
+        status                   = TRUE;
+        fpgaUpdateRegisterStatus = 0;
+        enqueue( FPGA_WRITE_UPDATE_REG );
     }
 
     return status;
@@ -351,7 +381,35 @@
     }
 }
 
+static void processFPGAReceivedData( void )
+{
+    // Capture the read values
+    switch( fpgaJobsQStatus.fpgaCurrentJob )
+    {
+        case FPGA_READ_HEADER:
+            memcpy( &fpgaHeader, &fpgaReadResponseBuffer[ FPGA_READ_RSP_HDR_LEN ], sizeof( FPGA_HEADER_T ) );
+            break;
 
+        case FPGA_READ_UPDATE_REG:
+            fpgaUpdateRegisterStatus = fpgaReadResponseBuffer[ FPGA_UPDATE_REQUEST_INDEX ];
+            break;
+
+        case FPGA_CHECK_ERASE_FIFO_STATUS:
+            break;
+
+        case FPGA_CHECK_FLASH_READY_STATUS:
+            break;
+
+        case FPGA_CHECK_FIFO_COUNT:
+            break;
+
+        default:
+            // Do nothing
+            break;
+    }
+}
+
+
 static void setupDMAForReadResp( U32 bytes2Receive )
 {
     // Verify # of bytes does not exceed buffer length
@@ -609,18 +667,8 @@
             // Does the FPGA response CRC check out?
             if ( crc == crc16( fpgaReadResponseBuffer, rspSize ) )
             {
-                // Capture the read values
-                switch( fpgaJobsQStatus.fpgaCurrentJob )
-                {
-                    case FPGA_READ_HEADER:
-                        memcpy( &fpgaHeader, &fpgaReadResponseBuffer[ FPGA_READ_RSP_HDR_LEN ], sizeof( FPGA_HEADER_T ) );
-                        break;
+                processFPGAReceivedData();
 
-                    case FPGA_READ_UPDATE_REG:
-                        fpgaUpdateRegisterStatus = fpgaReadResponseBuffer[ FPGA_UPDATE_REQUEST_INDEX ];
-                        break;
-                }
-
                 fpgaJobsQStatus.fpgaCommRead = FPGA_COMM_IDLE;
                 state                        = FPGA_IDLE_STATE;
             }