Index: firmware/App/Services/Download.c
===================================================================
diff -u -rab214e8ea52d8433b7cee58f5aaff49fc759310d -rf6fd9dfd1a30412e237ff45ebee44854f0e2d4b2
--- firmware/App/Services/Download.c	(.../Download.c)	(revision ab214e8ea52d8433b7cee58f5aaff49fc759310d)
+++ firmware/App/Services/Download.c	(.../Download.c)	(revision f6fd9dfd1a30412e237ff45ebee44854f0e2d4b2)
@@ -67,6 +67,7 @@
 } SW_UPDATE_FFU_BROADCAST_STATUS_T;
 
 static SW_UPDATE_RCV_STATUS_T SWUpdateRCVStatus;                                                          ///< Software update receive status.
+static SW_UPDATE_CMD_STATUS_T SWUpdateCmdStatus;                                                          ///< Software update command status.
 static U32 fpgaPayloadLengthBytes;                                                                        ///< FPGA payload length in bytes.
 static SW_UPDATE_SPECS_T SWUpdateSpecs;                                                                   ///< Software update specifications.
 static U32 SWUpdateLastBroadcastTimeMS;                                                                   ///< Software update last broadcast time stamp in milliseconds.
@@ -164,12 +165,12 @@
  * @param ack or nack status to be sent
  * @return none
  *************************************************************************/
-void sendFPGAAckNackStatus( ACK_NACK_STATUS_T ackNackStatus )
+void sendFPGAAckNackStatus( ACK_NACK_STATUS_T ackNackStatus, BOOL isAckForUpdate )
 {
-    SW_UPDATE_CAN_MAIL_BOX_T thisStackMailBox = RECEIVE_MSG_ID[ BL_STACK_ID ][ FW_STACKS_RCV_MAIL_BOX_INDEX ];
+    U16 msgID = ( TRUE == isAckForUpdate ? SWUpdateRCVStatus.msgID : SWUpdateCmdStatus.msgID );
 
-    prepareAndSendBootLoaderResponseMessage( SWUpdateRCVStatus.msgID, ackNackStatus );
-    clearCommBuffer( thisStackMailBox );
+    prepareAndSendBootLoaderResponseMessage( msgID, ackNackStatus );
+    clearSWUpdateBuffer();
 }
 
 /*********************************************************************//**
@@ -215,7 +216,7 @@
     if ( TRUE == isMessageComplete( mailBox ) )
     {
         // If the command buffer has been received, get it from the comm buffer and process it
-        SW_UPDATE_CMD_STATUS_T SWUpdateCmdStatus;
+        //SW_UPDATE_CMD_STATUS_T SWUpdateCmdStatus;
 
         getCommBuffer( mailBox, (U08*)&SWUpdateCmdStatus, sizeof( SW_UPDATE_CMD_STATUS_T ) );
 
@@ -242,8 +243,12 @@
             }
         }
 
-        // Send the result of the command received
-        prepareAndSendBootLoaderResponseMessage( msgID, ackStatus );
+        if ( SWUpdateSpecs.dest != UPDATE_FPGA )
+        {
+            // Send the result of the command received
+            prepareAndSendBootLoaderResponseMessage( msgID, ackStatus );
+        }
+
         clearCommBuffer( mailBox );
     }
 }
@@ -275,7 +280,7 @@
         payloadLength = SWUpdateRCVStatus.updatePayloadLen;
 
         // Create a local buffer and copy the header into the buffer excluding the CRC so only 4 bytes of ID, Destination, and payload length
-        memcpy( bufferWithNoCRC, (U08*)&SWUpdateRCVStatus, sizeof( U32 ) );
+        memcpy( &bufferWithNoCRC, (U08*)&SWUpdateRCVStatus, sizeof( U32 ) );
         // Copy the entire update buffer that is going either to firmware or FPGA into the local buffer.
         memcpy( &bufferWithNoCRC[ sizeof( U32 ) ], SWUpdateRCVStatus.SWUpdateBuffer, payloadLength );
         // Calculate the CRC of the local copied buffer and compare it against the message CRC
@@ -288,31 +293,29 @@
             BOOL test = TRUE; // TODO remove for testing
         }
 
-
         if ( TRUE == hasCRCPassed )
         {
             // CRC passed, call the corresponding the handlers to update either FPGA or firmware
             switch ( SWUpdateSpecs.dest )
             {
                 case UPDATE_FIRMWARE:
                     ackStatus = handleFirmwareUpdate();
-                    // Send the ack/nack status immediately because the other processes are blocked
+                    // For firmware update, send the ack status since in firmware flash all the other processes are halted
                     prepareAndSendBootLoaderResponseMessage( msgID, ackStatus );
+                    clearCommBuffer( mailBox );
+                    clearSWUpdateBuffer();
                     break;
 
                 case UPDATE_FPGA:
                     ackStatus = handleFPGAUpdate();
-                    // Do not send the ack/nack immediately. FPGA takes longer to respond and it will through other functions
+                    // For FPGA update, do not send the ack status immediately since the jobs are queued
                     break;
 
                 default:
                     // Do nothing
                     break;
             }
         }
-
-        clearCommBuffer( mailBox );
-        clearSWUpdateBuffer();
     }
 }
 
@@ -418,27 +421,36 @@
 static ACK_NACK_STATUS_T handleFPGAUpdate( void )
 {
     ACK_NACK_STATUS_T ackStatus = NACK;
+    BOOL status                 = FALSE;
 
-    // TODO why the firmware handler is slightly different? Make them consistent.
+//#define SW_UPDATE_FINAL_MSG_INDEX 0xFFFF // TODO: Remove. NOTE: once FPGA is ready investigate and remove 0xFFFF
 
-#define SW_UPDATE_FINAL_MSG_INDEX 0xFFFF // TODO: Remove. NOTE: once FPGA is ready investigate and remove 0xFFFF
-
-    if ( SWUpdateRCVStatus.updatePayloadLen != SW_UPDATE_FINAL_MSG_INDEX )
-    {
+    //if ( SWUpdateRCVStatus.updatePayloadLen != SW_UPDATE_FINAL_MSG_INDEX ) // TODO remove
+    //{
         fpgaPayloadLengthBytes = SWUpdateRCVStatus.updatePayloadLen;
         REMOVETHEVAR += fpgaPayloadLengthBytes;
 
-        signalFPGAToWriteToFlash( SWUpdateRCVStatus.SWUpdateBuffer, fpgaPayloadLengthBytes );
+        status = signalFPGAToWriteToFlash( SWUpdateRCVStatus.SWUpdateBuffer, fpgaPayloadLengthBytes );
 
         if ( fpgaPayloadLengthBytes < SW_UPDATE_FLASH_BUFFER_SIZE )
         {
+            // TODO this should be a command and not the last buffer because what if the last payload is 256 bytes?
             signalFPGAToSelfConfigure();
         }
-    }
-    else
-    {
-        prepareAndSendBootLoaderResponseMessage( SWUpdateRCVStatus.msgID, ACK );
-    }
 
+        if ( TRUE == status )
+        {
+            // The update payload has been enqueued successfully, clear the buffers
+            SW_UPDATE_CAN_MAIL_BOX_T thisStackMailBox = RECEIVE_MSG_ID[ BL_STACK_ID ][ FW_STACKS_RCV_MAIL_BOX_INDEX ];
+
+            clearCommBuffer( thisStackMailBox );
+            //clearSWUpdateBuffer(); // TODO remove?
+        }
+    //}
+    //else
+    //{
+    //    prepareAndSendBootLoaderResponseMessage( SWUpdateRCVStatus.msgID, ACK );
+    //}
+
     return ackStatus;
 }