diff --git a/ecmc_plugin_grbl/ecmcGrbl.cpp b/ecmc_plugin_grbl/ecmcGrbl.cpp
index 394e17b..d5ddfbc 100644
--- a/ecmc_plugin_grbl/ecmcGrbl.cpp
+++ b/ecmc_plugin_grbl/ecmcGrbl.cpp
@@ -281,12 +281,16 @@ void ecmcGrbl::doWriteWorker() {
         char c = ecmc_get_char_from_grbl_tx_buffer();
         reply += c;
         if(c == '\n'&& reply.length() > 1) {
-          printf("Reply from grbl: %s\n",reply.c_str());
-          //if not below strinsg then push messages.. how to handle?!
-          //#define ECMC_PLUGIN_GRBL_GRBL_OK_STRING "ok"
-          //#define ECMC_PLUGIN_GRBL_GRBL_ERR_STRING "error"
-          //Example "error:2"
-          break;
+          if(reply.find(ECMC_PLUGIN_GRBL_GRBL_OK_STRING) != std::string::npos) {
+            printf("Reply OK from grbl: %s\n",reply.c_str());
+            break;
+          } else if(reply.find(ECMC_PLUGIN_GRBL_GRBL_ERR_STRING) != std::string::npos) {
+            printf("Reply ERROR from grbl: %s\n",reply.c_str());
+            break;
+          } else {
+            // lkeep waiting
+            printf("Non protocol value: %s\n",reply.c_str());
+          }
         }
       }
     }
diff --git a/iocsh/test.script b/iocsh/test.script
index 9944000..5cd5997 100644
--- a/iocsh/test.script
+++ b/iocsh/test.script
@@ -68,6 +68,7 @@ epicsEnvUnset(ECMC_PLUGIN_CONFIG)
 ecmcGrblAddCommand("G1X20Y20F180");
 ecmcGrblAddCommand("G2X0Y0R20");
 ecmcGrblAddCommand("G0X10Y10");
+ecmcGrblAddCommand("G2X0adadsdY0R20");
 ecmcGrblAddCommand("G1X0Y20");
 ecmcGrblAddCommand("G1X10Y0F360");
 ecmcGrblAddCommand("G4P1");