Index: firmware/App/Modes/ModeFill.c
===================================================================
diff -u -r499e5de29e706d09f79ba22511068990c4044e84 -r4fa5628781e2b420cad6afc42a4b754dd484b997
--- firmware/App/Modes/ModeFill.c	(.../ModeFill.c)	(revision 499e5de29e706d09f79ba22511068990c4044e84)
+++ firmware/App/Modes/ModeFill.c	(.../ModeFill.c)	(revision 4fa5628781e2b420cad6afc42a4b754dd484b997)
@@ -1,4 +1,4 @@
-/**************************************************************************
+/**********************************************************************//**
  *
  *  Copyright (c) 2019-2020 Diality Inc. - All Rights Reserved.
  *
@@ -14,151 +14,170 @@
  *
  **************************************************************************/
 
-#include "ModeFill.h"
 #include "OperationModes.h"
 #include "Timers.h"
+#include "ModeFill.h"
 
 #ifdef RM46_EVAL_BOARD_TARGET
     #include "CPLD.h"
 #endif
 
-// ********** private data **********
-static volatile enum FillModeSubModes
-{
-    CHECK_INLET_WATER = 0,
-    CREATE_PRODUCT_WATER,
-    DIALYSATE_PRODUCTION,
-    DELIVER_DIALYSATE
-} fillCurrentSubMode;
+// ********** private definitions **********
 
-static U32 timer;
-static U16 toggle_counter = 0;
-
 #define QUARTER_SECOND   250
 #define HALF_SECOND      500
 
+typedef enum Fill_Mode_States
+{
+    FILL_MODE_STATE_START = 0,
+    FILL_MODE_STATE_CHECK_INLET_WATER,
+    FILL_MODE_STATE_CREATE_PRODUCT_WATER,
+    FILL_MODE_STATE_DIALYSATE_PRODUCTION,
+    FILL_MODE_STATE_DELIVER_DIALYSATE,
+    NUM_OF_FILL_MODE_STATES
+} FILL_MODE_STATE_T;
 
+// ********** private data **********
+
+static FILL_MODE_STATE_T fillState;
+
 // ********** private function prototypes **********
 
-/*************************************************************************
+static FILL_MODE_STATE_T handleCheckInletWaterState( void );
+static FILL_MODE_STATE_T handleCreateProductWaterState( void );
+static FILL_MODE_STATE_T handleDialysateProductionState( void );
+static FILL_MODE_STATE_T handleDeliverDialysateState( void );
+
+/*********************************************************************//**
  * @brief initFillMode
  * The initFillMode function initializes the Fill Mode module.
  * @details
  * Inputs : none
  * Outputs : Fill Mode module initialized.
- * @param none
  * @return none
  *************************************************************************/
 void initFillMode( void )
 {
-    fillCurrentSubMode = CHECK_INLET_WATER;
-    timer = getMSTimerCount();
+    fillState = FILL_MODE_STATE_START;
 }
 
-/*************************************************************************
+/*********************************************************************//**
  * @brief transitionToFillMode
  * The transitionToFillMode function prepares for transition to \n
  * fill mode.
  * @details
  * Inputs : none
- * Outputs :
- * @param none
+ * Outputs : fillState
  * @return none
  *************************************************************************/
 void transitionToFillMode( void )
 {
-    fillCurrentSubMode = CHECK_INLET_WATER;
-    timer = getMSTimerCount();
-    toggle_counter = 0;
+    fillState = FILL_MODE_STATE_START;
 }
 
-/*************************************************************************
+/*********************************************************************//**
  * @brief execFillMode
  * The execFillMode function executes the Fill Mode state machine.
  * @details
- * Inputs : none
- * Outputs :
- * @param none
+ * Inputs : fillState
+ * Outputs : fillState
  * @return none
  *************************************************************************/
 void execFillMode( void )
 {
+    // execute current Fill state
+    switch ( fillState )
+    {
+        case FILL_MODE_STATE_START:
+            fillState = FILL_MODE_STATE_CHECK_INLET_WATER;
+            break;
 
-    switch (fillCurrentSubMode){
+        case FILL_MODE_STATE_CHECK_INLET_WATER:
+            fillState = handleCheckInletWaterState();
+            break;
 
-        case CHECK_INLET_WATER:
+        case FILL_MODE_STATE_CREATE_PRODUCT_WATER:
+            fillState = handleCreateProductWaterState();
+            break;
 
-            // We check every half second to toggle LED
-            if (TRUE == didTimeout(timer, QUARTER_SECOND))
-            {
-                timer = getMSTimerCount(); // Reset timer
-                #ifdef RM46_EVAL_BOARD_TARGET
-                    toggleUserLED();
-                #endif
-                toggle_counter++;
-            }
+        case FILL_MODE_STATE_DIALYSATE_PRODUCTION:
+            fillState = handleDialysateProductionState();
+            break;
 
-            if (toggle_counter == 8)
-            {
-                toggle_counter = 0;
+        case FILL_MODE_STATE_DELIVER_DIALYSATE:
+            fillState = handleDeliverDialysateState();
+            break;
 
-                // switch to submode
-                fillCurrentSubMode = CREATE_PRODUCT_WATER;
-            }
-
+        default:
+            // TODO - s/w fault
             break;
+    }
+}
 
-        case CREATE_PRODUCT_WATER:
+/*********************************************************************//**
+ * @brief
+ * The handleCheckInletWaterState function executes the Check Inlet Water \n
+ * state of the Fill Mode state machine.
+ * @details
+ * Inputs : none
+ * Outputs :
+ * @param none
+ * @return the next state
+ *************************************************************************/
+static FILL_MODE_STATE_T handleCheckInletWaterState( void )
+{
+    FILL_MODE_STATE_T result = FILL_MODE_STATE_CHECK_INLET_WATER;
 
-            // We check every half second to toggle LED
-            if (TRUE == didTimeout(timer, HALF_SECOND))
-            {
-                timer = getMSTimerCount(); // Reset timer
-                #ifdef RM46_EVAL_BOARD_TARGET
-                    toggleUserLED();
-                #endif
-                toggle_counter++;
-            }
+    return result;
+}
 
-            if (toggle_counter == 4)
-            {
-                toggle_counter = 0;
+/*********************************************************************//**
+ * @brief
+ * The handleCreateProductWaterState function executes the Create Product \n
+ * Water state of the Fill Mode state machine.
+ * @details
+ * Inputs : none
+ * Outputs :
+ * @param none
+ * @return the next state
+ *************************************************************************/
+static FILL_MODE_STATE_T handleCreateProductWaterState( void )
+{
+    FILL_MODE_STATE_T result = FILL_MODE_STATE_CREATE_PRODUCT_WATER;
 
-                // switch to submode
-                fillCurrentSubMode = DIALYSATE_PRODUCTION;
-            }
-            break;
+    return result;
+}
 
-        case DIALYSATE_PRODUCTION:
-            // We check every half second to toggle LED
-            if (TRUE == didTimeout(timer, HALF_SECOND + QUARTER_SECOND))
-            {
-                timer = getMSTimerCount(); // Reset timer
-                #ifdef RM46_EVAL_BOARD_TARGET
-                    toggleUserLED();
-                #endif
-                toggle_counter++;
-            }
+/*********************************************************************//**
+ * @brief
+ * The handleDialysateProductionState function executes the Dialysate Production \n
+ * state of the Fill Mode state machine.
+ * @details
+ * Inputs : none
+ * Outputs :
+ * @param none
+ * @return the next state
+ *************************************************************************/
+static FILL_MODE_STATE_T handleDialysateProductionState( void )
+{
+    FILL_MODE_STATE_T result = FILL_MODE_STATE_DIALYSATE_PRODUCTION;
 
-            if (toggle_counter == 4)
-            {
-                toggle_counter = 0;
+    return result;
+}
 
-                // switch to submode
-                fillCurrentSubMode = DELIVER_DIALYSATE;
-            }
-            break;
+/*********************************************************************//**
+ * @brief
+ * The handleDeliverDialysateState function executes the Deliver Dialysate \n
+ * state of the Fill Mode state machine.
+ * @details
+ * Inputs : none
+ * Outputs :
+ * @param none
+ * @return the next state
+ *************************************************************************/
+static FILL_MODE_STATE_T handleDeliverDialysateState( void )
+{
+    FILL_MODE_STATE_T result = FILL_MODE_STATE_DELIVER_DIALYSATE;
 
-        case DELIVER_DIALYSATE:
-
-            if(toggle_counter == 0)
-            {
-                #ifdef RM46_EVAL_BOARD_TARGET
-                    setUserLED(TRUE);
-                #endif
-                toggle_counter++;
-            }
-            break;
-    }
+    return result;
 }
-