diff --git a/motion_control.c b/motion_control.c
index f4b66cb..f6b7037 100644
--- a/motion_control.c
+++ b/motion_control.c
@@ -320,6 +320,7 @@ void mc_homing_cycle()
       bit_true_atomic(sys.execute, EXEC_CRIT_EVENT);
     } else {
       perform_pull_off = 0;
+      probe_finalize();
     }
   }
   protocol_execute_runtime();   // Check and execute run-time commands
diff --git a/probe.c b/probe.c
index 7d45fe6..3623f7b 100644
--- a/probe.c
+++ b/probe.c
@@ -50,6 +50,12 @@ uint8_t probe_errors_enabled(uint8_t mode) {
   return !(mode & PROBE_NO_ERROR);
 }
 
+void probe_finalize() {
+  sys.probe_state = PROBE_OFF;
+  memcpy(sys.probe_position, sys.position, sizeof(float)*N_AXIS);
+  bit_true(sys.execute, EXEC_FEED_HOLD);
+}
+
 // Monitors probe pin state and records the system position when detected. Called by the
 // stepper ISR per ISR tick.
 // NOTE: This function must be extremely efficient as to not bog down the stepper ISR.
@@ -57,9 +63,7 @@ void probe_state_monitor()
 {
   if (sys.probe_state != PROBE_OFF) {
     if (probe_get_state(sys.probe_state)) {
-      sys.probe_state = PROBE_OFF;
-      memcpy(sys.probe_position, sys.position, sizeof(float)*N_AXIS);
-      bit_true(sys.execute, EXEC_FEED_HOLD);
+      probe_finalize();
     }
   }
 }
diff --git a/probe.h b/probe.h
index 5a2006d..194e56e 100644
--- a/probe.h
+++ b/probe.h
@@ -40,6 +40,8 @@ uint8_t probe_get_state(uint8_t mode);
 
 uint8_t probe_errors_enabled(uint8_t mode);
 
+void probe_finalize();
+
 // Monitors probe pin state and records the system position when detected. Called by the
 // stepper ISR per ISR tick.
 void probe_state_monitor();