Index: firmware/App/Services/Download.c
===================================================================
diff -u -rf6fd9dfd1a30412e237ff45ebee44854f0e2d4b2 -r621896c8a4cc9204a12a7243ccef729c37e50c52
--- firmware/App/Services/Download.c	(.../Download.c)	(revision f6fd9dfd1a30412e237ff45ebee44854f0e2d4b2)
+++ firmware/App/Services/Download.c	(.../Download.c)	(revision 621896c8a4cc9204a12a7243ccef729c37e50c52)
@@ -236,16 +236,31 @@
             SWUpdateSpecs.dest  = (SW_UPDATE_DESINTATION_T)SWUpdateCmdStatus.destination;
             ackStatus           = ACK;
 
-            if ( UPDATE_FPGA == SWUpdateSpecs.dest ) // TODO once verify command is received for FPGA, make sure the prepare signal is not sent again
+            if ( UPDATE_FPGA == SWUpdateSpecs.dest )
             {
-                // If the update destination is FPGA, signal FPGA to prepare for the update.
-                signalFPGAToPrepareForUpdate();
+                switch ( SWUpdateSpecs.cmd )
+                {
+                    case UPDATE_CMD_START:
+                        // If the update destination is FPGA, signal FPGA to prepare for the update.
+                        signalFPGAToPrepareForUpdate();
+                        break;
+
+                    case UPDATE_CMD_CONFIG_FPGA:
+                        // Signal FPGA to self configure after the update is finished.
+                        signalFPGAToSelfConfigure();
+                        break;
+
+                    default:
+                        //  Do nothing for the rest of the commands
+                        break;
+                }
             }
         }
 
         if ( SWUpdateSpecs.dest != UPDATE_FPGA )
         {
-            // Send the result of the command received
+            // Send the result of the command received for firmware.
+            // FPGA is delayed and it takes a while to prepare it so ack is not sent immediately
             prepareAndSendBootLoaderResponseMessage( msgID, ackStatus );
         }
 
@@ -309,13 +324,19 @@
                 case UPDATE_FPGA:
                     ackStatus = handleFPGAUpdate();
                     // For FPGA update, do not send the ack status immediately since the jobs are queued
+                    // For consistency we still receive the ack status but it is not sent.
                     break;
 
                 default:
                     // Do nothing
                     break;
             }
         }
+        else
+        {
+            // CRC did not pass send a nack
+            prepareAndSendBootLoaderResponseMessage( msgID, ackStatus );
+        }
     }
 }
 
@@ -423,34 +444,27 @@
     ACK_NACK_STATUS_T ackStatus = NACK;
     BOOL status                 = FALSE;
 
-//#define SW_UPDATE_FINAL_MSG_INDEX 0xFFFF // TODO: Remove. NOTE: once FPGA is ready investigate and remove 0xFFFF
+    fpgaPayloadLengthBytes = SWUpdateRCVStatus.updatePayloadLen;
+    REMOVETHEVAR += fpgaPayloadLengthBytes;
 
-    //if ( SWUpdateRCVStatus.updatePayloadLen != SW_UPDATE_FINAL_MSG_INDEX ) // TODO remove
-    //{
-        fpgaPayloadLengthBytes = SWUpdateRCVStatus.updatePayloadLen;
-        REMOVETHEVAR += fpgaPayloadLengthBytes;
+    status = 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();
-        }
-
-        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
+    // TODO remove this code
+    //if ( fpgaPayloadLengthBytes < SW_UPDATE_FLASH_BUFFER_SIZE )
     //{
-    //    prepareAndSendBootLoaderResponseMessage( SWUpdateRCVStatus.msgID, ACK );
+    //    // TODO this should be a command and not the last buffer because what if the last payload is 256 bytes?
+    //    signalFPGAToSelfConfigure();
     //}
 
+    if ( TRUE == status )
+    {
+        // The update payload has been enqueued successfully, clear the raw buffer but keep the receive buffer since the enqueue
+        // to write to the FPGA buffer is delayed until the FPGA is ready and there is enough bytes available in the FIFO.
+        // The receive buffer is cleared once the write to FPGA is scheduled.
+        SW_UPDATE_CAN_MAIL_BOX_T thisStackMailBox = RECEIVE_MSG_ID[ BL_STACK_ID ][ FW_STACKS_RCV_MAIL_BOX_INDEX ];
+
+        clearCommBuffer( thisStackMailBox );
+    }
+
     return ackStatus;
 }