Index: firmware/App/Modes/ModeStandby.c
===================================================================
diff -u -rdeef095c63fe86de42a7e052e1b9985b0118b02e -r9af6fc3e5afc442a877bd5e23ecfa6872a3ad5a4
--- firmware/App/Modes/ModeStandby.c	(.../ModeStandby.c)	(revision deef095c63fe86de42a7e052e1b9985b0118b02e)
+++ firmware/App/Modes/ModeStandby.c	(.../ModeStandby.c)	(revision 9af6fc3e5afc442a877bd5e23ecfa6872a3ad5a4)
@@ -5,4 +5,159 @@
  *      Author: fw
  */
 
+#include "string.h"     // for memset
+#include "sys_core.h"   // To disable RAM and Flash ECC
+#include "sys_mpu.h"    // To disable MPU
 
+#include "CommBuffers.h"
+#include "ModeStandby.h"
+#include "OperationModes.h"
+#include "Timers.h"
+
+
+#define WAIT_FOR_UPDATE_FROM_UI_MS                  1000
+
+static SW_UPDATE_CMD_STATUS_T SWUpdateCmdStatus;
+static MODE_STANDBY_STATE_T standbyCurrentState;
+static U32 waitForUpdateMsgStarTimeMS;
+
+static const U32 jumpAd = (U32)FIRMWARE_START_ADDRESS;
+
+static MODE_STANDBY_STATE_T handleStandbyModeCheckForUpdateState( void );
+static MODE_STANDBY_STATE_T handleStandbyModeCheckFWAndFPGAImages( void );
+static MODE_STANDBY_STATE_T handleStandbyModeIdleState( void );
+
+static void jumpToApplication( void );
+
+
+void initStandbyMode( void )
+{
+    standbyCurrentState        = STANDBY_CHECK_FOR_UPDATE_STATE;
+    waitForUpdateMsgStarTimeMS = getMSTimerCount();
+
+    memset( &SWUpdateCmdStatus, 0x0, sizeof( SW_UPDATE_CMD_STATUS_T ) );
+}
+
+U32 transitionToStandbyMode( void )
+{
+    initStandbyMode();
+
+    return 0;
+}
+
+U32 execStandbyMode( void )
+{
+    getSWUpdateCmdStatus( &SWUpdateCmdStatus );
+
+    // If the bootloader is the standby mode and and update request is received at any time, request a transition to update mode
+    // TODO what if we are in the check image process, should we transition to update upon the request?
+    if ( ( UPDATE_CMD_START == (SW_UPDATE_CMD_T)SWUpdateCmdStatus.updateCmd ) /*|| ( TRUE ) This is FPGA update register for later */ )
+    {
+        // TODO check for the FPGA update request register
+        // TODO if we are here because of the FPGA register, clear it so it won't be called again.
+        requestNewOperationMode( MODE_UPDATE );
+    }
+
+    switch( standbyCurrentState )
+    {
+        case STANDBY_CHECK_FOR_UPDATE_STATE:
+            standbyCurrentState = handleStandbyModeCheckForUpdateState();
+            break;
+
+        case STANDBY_CHECK_FW_AND_FPGA_IMAGES_STATE:
+            standbyCurrentState = handleStandbyModeCheckFWAndFPGAImages();
+            break;
+
+        case STANDBY_IDLE_STATE:
+            standbyCurrentState = handleStandbyModeIdleState();
+            break;
+
+        default:
+            // Do nothing
+            break;
+    }
+
+    return standbyCurrentState;
+}
+
+
+static MODE_STANDBY_STATE_T handleStandbyModeCheckForUpdateState( void )
+{
+    MODE_STANDBY_STATE_T state = STANDBY_CHECK_FOR_UPDATE_STATE;
+
+    switch ( SWUpdateCmdStatus.updateCmd )
+    {
+        case UPDATE_CMD_ABORT:
+            state = STANDBY_IDLE_STATE;
+            break;
+
+        case UPDATE_CMD_VERIFY:
+            state = STANDBY_CHECK_FW_AND_FPGA_IMAGES_STATE;
+            break;
+
+        default:
+            // Do nothing
+            break;
+    }
+
+    clearSWUpdateBuffer( SW_UPDATE_COMMAD );
+
+    if ( TRUE == didTimeout( waitForUpdateMsgStarTimeMS, 20000 /*WAIT_FOR_UPDATE_FROM_UI_MS*/ ) ) // TODO a high number if timeout for development
+    {
+        state = STANDBY_CHECK_FW_AND_FPGA_IMAGES_STATE;
+    }
+
+    return state;
+}
+
+static MODE_STANDBY_STATE_T handleStandbyModeCheckFWAndFPGAImages( void )
+{
+    MODE_STANDBY_STATE_T state = STANDBY_CHECK_FW_AND_FPGA_IMAGES_STATE;
+    BOOL areImagesOk           = FALSE;
+
+    // TODO check the FW integrity and FPGA version or something else
+    // TODO for testing only
+    areImagesOk = TRUE;
+    // TODO for testing only remove
+
+    if ( TRUE == areImagesOk )
+    {
+        // All good, jump to application
+        jumpToApplication();
+    }
+    else
+    {
+        // If either of the images (firmware or FPGA) failed, go to the idle state until another update request is received
+        state = STANDBY_IDLE_STATE;
+    }
+
+    return state;
+}
+
+static MODE_STANDBY_STATE_T handleStandbyModeIdleState( void )
+{
+    MODE_STANDBY_STATE_T state = STANDBY_IDLE_STATE;
+
+    // This state does nothing and waits until another update request is received. This could be because the update was aborted or
+    // the firmware or FPGA have bad images. So we cannot jump to the application.
+
+    return state;
+}
+
+static void jumpToApplication( void )
+{
+    // TODO uncomment
+    // Disable various memory protections
+    _coreDisableRamEcc_();
+    _coreDisableFlashEcc_();
+    _mpuDisable_();
+
+    // Disable all interrupts
+    _disable_interrupt_();
+    _disable_IRQ_interrupt_();
+    _disable_FIQ_interrupt_();
+
+    ((void (*)(void))jumpAd)();
+    while(1);
+}
+