diff --git a/grbl_machine.cpp b/grbl_machine.cpp
index d75f1de..977da0b 100644
--- a/grbl_machine.cpp
+++ b/grbl_machine.cpp
@@ -99,13 +99,10 @@ void grbl::machine::run_program(const grbl::program& pgm) {
 }
 
 void grbl::machine::check_program(const grbl::program& pgm) {
-    std::cout << "checking program (" << pgm.filename << ") with " << pgm.number_of_instructions() << " instructions" << std::endl;
-
     running_program = pgm;
-    entered_check_mode = false;
-    executed_instructions = 0;
-    state = grbl_machine_state::check_program;
-    pipe->send("$C");
+    std::cout << "checking program (" << running_program.filename << ") with " << running_program.number_of_instructions()
+              << " instructions" << std::endl;
+    switch_to_state(grbl_machine_state::check_program);
 }
 
 std::string grbl::status_to_string(const grbl::machine_status& status) {
@@ -605,8 +602,11 @@ void grbl::machine::reset_machine_state() {
 }
 
 void grbl::machine::switch_to_state(grbl::grbl_machine_state new_state) {
+    std::cout << "Switching from state " << (int) state << " to " << (int) new_state << std::endl;
+    std::cout << "Exiting from state" << std::endl;
     states[state]->on_exit(this);
     state = new_state;
+    std::cout << "Entering to state" << std::endl;
     states[state]->on_enter(this);
 }
 
@@ -783,15 +783,94 @@ void grbl::machine_state_check_program::on_disconnected(machine *m) {
 }
 
 void grbl::machine_state_check_program::on_enter(grbl::machine *m) {
+    cnc = m;
 
+    stage = check_stage::start;
+    check_failed = false;
+    check_error = 0;
+    move_to_next_check_stage();
 }
 
 void grbl::machine_state_check_program::on_exit(grbl::machine *m) {
+}
 
+void grbl::machine_state_check_program::move_to_next_check_stage() {
+    switch (stage) {
+        case check_stage::start:
+            while (cnc->awaiting_responses > 0);
+            cnc->pipe->send("$C");
+            cnc->awaiting_responses++;
+
+            cnc->executed_instructions = 0;
+            stage = check_stage::enable_check_mode;
+            break;
+        case check_stage::enable_check_mode:
+            continue_program();
+            stage = check_stage::run_program;
+            break;
+        case check_stage::run_program:
+            if (check_failed || !continue_program()) {
+                stage = check_stage::disable_check_mode;
+
+                while (cnc->awaiting_responses > 0);
+                std::cout << "disabling $C mode" << std::endl;
+                cnc->pipe->send("$C");
+            }
+            break;
+        case check_stage::disable_check_mode:
+            if (check_failed) {
+                cnc->listener->on_check_completed(false, cnc->executed_instructions - 1, check_error);
+            } else {
+                cnc->listener->on_check_completed(true, 0, 0);
+            }
+            cnc->switch_to_state(grbl_machine_state::idle);
+            break;
+    }
 }
 
 void grbl::machine_state_check_program::on_line_received(std::string line) {
+    if (starts_with(line, "ok")) {
+        if (cnc->awaiting_responses > 0) {
+            cnc->awaiting_responses--;
+        }
+        move_to_next_check_stage();
+    } else if (starts_with(line, "error")) {
+        if (cnc->awaiting_responses > 0) {
+            cnc->awaiting_responses--;
+        }
+        check_failed = true;
+        check_error = std::stoi(line.substr(6));
+        move_to_next_check_stage();
 
+    } else if (starts_with(line, "Grbl")) {
+        cnc->listener->on_banner(line);
+        cnc->reset_machine_state();
+    } else if (starts_with(line, "<")) {
+        cnc->last_report = parse_status_report(line, cnc->last_report);
+        cnc->listener->on_realtime_status_report(cnc->last_report);
+    } else if (starts_with(line, "[MSG:")) {
+        cnc->listener->on_message(line.substr(5, line.size() - 6));
+    } else if (starts_with(line, "ALARM:")) {
+        cnc->listener->on_alarm(std::stoi(line.substr(6)));
+    }
+}
+
+bool grbl::machine_state_check_program::continue_program() {
+    if (cnc->executed_instructions >= cnc->running_program.number_of_instructions())
+        return false;
+
+    instruction to_send;
+    do {
+        to_send = cnc->running_program.instruction_at(cnc->executed_instructions++);
+    } while (to_send.type != instruction_type::gcode && cnc->executed_instructions < cnc->running_program.number_of_instructions());
+
+    if (to_send.type == instruction_type::gcode) {
+        cnc->pipe->send(to_send.command);
+        cnc->awaiting_responses++;
+        return true;
+    }
+
+    return false;
 }
 
 
diff --git a/grbl_machine.h b/grbl_machine.h
index c11f2a6..b2ec59b 100644
--- a/grbl_machine.h
+++ b/grbl_machine.h
@@ -124,6 +124,13 @@ enum class init_stage {
     fetch_parameters
 };
 
+enum class check_stage {
+    start,
+    enable_check_mode,
+    run_program,
+    disable_check_mode
+};
+
 struct machine;
 
 struct machine_state {
@@ -171,6 +178,13 @@ struct machine_state_check_program : public machine_state {
     void on_enter(machine *m) override;
     void on_exit(machine *m) override;
     void on_line_received(std::string line) override;
+
+    check_stage stage = check_stage::start;
+    machine *cnc;
+    bool continue_program();
+    void move_to_next_check_stage();
+    bool check_failed;
+    size_t check_error;
 };
 
 struct machine_state_run_program : public machine_state {
@@ -245,7 +259,6 @@ protected:
     grbl_machine_state state = grbl_machine_state::disconnected;
     program running_program;
     size_t executed_instructions = 0;
-    bool entered_check_mode = false;
     void continue_program();
     void reset_machine_state();
 };