diff --git a/ecmc_plugin_grbl/ecmcGrbl.cpp b/ecmc_plugin_grbl/ecmcGrbl.cpp
new file mode 100644
index 0000000..aa06313
--- /dev/null
+++ b/ecmc_plugin_grbl/ecmcGrbl.cpp
@@ -0,0 +1,468 @@
+/*************************************************************************\
+* Copyright (c) 2019 European Spallation Source ERIC
+* ecmc is distributed subject to a Software License Agreement found
+* in file LICENSE that is included with this distribution. 
+*
+*  ecmcSocketCAN.cpp
+*
+*  Created on: Mar 22, 2020
+*      Author: anderssandstrom
+*      Credits to  https://github.com/sgreg/dynamic-loading 
+*
+\*************************************************************************/
+
+// Needed to get headers in ecmc right...
+#define ECMC_IS_PLUGIN
+
+#include <sstream>
+#include "ecmcSocketCAN.h"
+#include "ecmcPluginClient.h"
+#include "ecmcAsynPortDriver.h"
+#include "ecmcAsynPortDriverUtils.h"
+#include "epicsThread.h"
+
+// Start worker for socket read()
+void f_worker_read(void *obj) {
+  if(!obj) {
+    printf("%s/%s:%d: Error: Worker read thread ecmcSocketCAN object NULL..\n",
+            __FILE__, __FUNCTION__, __LINE__);
+    return;
+  }
+  ecmcSocketCAN * canObj = (ecmcSocketCAN*)obj;
+  canObj->doReadWorker();
+}
+
+// Start worker for socket connect()
+void f_worker_connect(void *obj) {
+  if(!obj) {
+    printf("%s/%s:%d: Error: Worker connect thread ecmcSocketCAN object NULL..\n",
+            __FILE__, __FUNCTION__, __LINE__);
+    return;
+  }
+  ecmcSocketCAN * canObj = (ecmcSocketCAN*)obj;
+  canObj->doConnectWorker();
+}
+
+/** ecmc ecmcSocketCAN class
+ * This object can throw: 
+ *    - bad_alloc
+ *    - invalid_argument
+ *    - runtime_error
+*/
+ecmcSocketCAN::ecmcSocketCAN(char* configStr,
+                             char* portName,
+                             int exeSampleTimeMs) {
+  // Init
+  cfgCanIFStr_    = NULL;
+  cfgDbgMode_     = 0;
+  cfgAutoConnect_ = 1;
+  destructs_      = 0;
+  socketId_       = -1;
+  connected_      = 0;
+  writeBuffer_    = NULL;
+  deviceCounter_  = 0;
+  refreshNeeded_  = 0;
+  errorCode_      = 0;
+  masterDev_      = NULL;
+  for(int i = 0; i<ECMC_CAN_MAX_DEVICES;i++) {
+    devices_[i] = NULL;
+  }
+
+  exeSampleTimeMs_ = exeSampleTimeMs;
+  
+  memset(&ifr_,0,sizeof(struct ifreq));
+  memset(&rxmsg_,0,sizeof(struct can_frame));
+  memset(&addr_,0,sizeof(struct sockaddr_can));
+
+  parseConfigStr(configStr); // Assigns all configs
+  // Check valid nfft
+  if(!cfgCanIFStr_ ) {
+    throw std::out_of_range("CAN inteface must be defined (can0, vcan0...).");
+  }
+
+  // Create worker thread for reading socket
+  std::string threadname = "ecmc." ECMC_PLUGIN_ASYN_PREFIX".read";
+  if(epicsThreadCreate(threadname.c_str(), 0, 32768, f_worker_read, this) == NULL) {
+    throw std::runtime_error("Error: Failed create worker thread for read().");
+  }
+
+  // Create worker thread for connecting socket
+  threadname = "ecmc." ECMC_PLUGIN_ASYN_PREFIX".connect";
+  if(epicsThreadCreate(threadname.c_str(), 0, 32768, f_worker_connect, this) == NULL) {
+    throw std::runtime_error("Error: Failed create worker thread for connect().");
+  }
+
+  if(cfgAutoConnect_) {
+    connectPrivate();
+  }
+  writeBuffer_ = new ecmcSocketCANWriteBuffer(socketId_, cfgDbgMode_);
+  initAsyn();
+}
+
+ecmcSocketCAN::~ecmcSocketCAN() {
+  // kill worker
+  destructs_ = 1;  // maybe need todo in other way..
+  doWriteEvent_.signal();
+  doConnectEvent_.signal();
+
+  for(int i = 0; i<ECMC_CAN_MAX_DEVICES;i++) {
+    delete devices_[i];
+  }
+
+}
+
+void ecmcSocketCAN::parseConfigStr(char *configStr) {
+
+  // check config parameters
+  if (configStr && configStr[0]) {    
+    char *pOptions = strdup(configStr);
+    char *pThisOption = pOptions;
+    char *pNextOption = pOptions;
+    
+    while (pNextOption && pNextOption[0]) {
+      pNextOption = strchr(pNextOption, ';');
+      if (pNextOption) {
+        *pNextOption = '\0'; /* Terminate */
+        pNextOption++;       /* Jump to (possible) next */
+      }
+      
+      // ECMC_PLUGIN_DBG_PRINT_OPTION_CMD (1/0)
+      if (!strncmp(pThisOption, ECMC_PLUGIN_DBG_PRINT_OPTION_CMD, strlen(ECMC_PLUGIN_DBG_PRINT_OPTION_CMD))) {
+        pThisOption += strlen(ECMC_PLUGIN_DBG_PRINT_OPTION_CMD);
+        cfgDbgMode_ = atoi(pThisOption);
+      }
+      
+      // ECMC_PLUGIN_CONNECT_OPTION_CMD (1/0)
+      if (!strncmp(pThisOption, ECMC_PLUGIN_CONNECT_OPTION_CMD, strlen(ECMC_PLUGIN_CONNECT_OPTION_CMD))) {
+        pThisOption += strlen(ECMC_PLUGIN_DBG_PRINT_OPTION_CMD);
+        cfgAutoConnect_ = atoi(pThisOption);
+      }
+
+      // ECMC_PLUGIN_IF_OPTION_CMD (Source string)
+      else if (!strncmp(pThisOption, ECMC_PLUGIN_IF_OPTION_CMD, strlen(ECMC_PLUGIN_IF_OPTION_CMD))) {
+        pThisOption += strlen(ECMC_PLUGIN_IF_OPTION_CMD);
+        cfgCanIFStr_=strdup(pThisOption);
+      }
+
+      pThisOption = pNextOption;
+    }    
+    free(pOptions);
+  }
+  if(!cfgCanIFStr_) { 
+    throw std::invalid_argument( "CAN interface not defined.");
+  }
+}
+
+// For connect commands over asyn or plc. let worker connect
+void ecmcSocketCAN::connectExternal() {
+  if(!connected_) {
+    doConnectEvent_.signal(); // let worker start
+  }
+}
+
+void ecmcSocketCAN::connectPrivate() {
+
+	if((socketId_ = socket(PF_CAN, SOCK_RAW, CAN_RAW)) == -1) {
+    throw std::runtime_error( "Error while opening socket.");		
+		return;
+	}
+
+	strcpy(ifr_.ifr_name, cfgCanIFStr_);
+	ioctl(socketId_, SIOCGIFINDEX, &ifr_);
+	
+	addr_.can_family  = AF_CAN;
+	addr_.can_ifindex = ifr_.ifr_ifindex;
+
+	printf("%s at index %d\n", cfgCanIFStr_, ifr_.ifr_ifindex);
+
+	if(bind(socketId_, (struct sockaddr *)&addr_, sizeof(addr_)) == -1) {		
+    throw std::runtime_error( "Error in socket bind.");
+    return;
+	}
+  connected_ = 1;
+}
+
+int ecmcSocketCAN::getConnected() {
+  return connected_;
+}
+
+// Read socket worker
+void ecmcSocketCAN::doReadWorker() {
+
+  while(true) {
+    
+    if(destructs_) {
+      break;
+    }
+
+    // Wait for new CAN frame 
+    int bytes = read(socketId_, &rxmsg_, sizeof(rxmsg_));
+    if(bytes == -1) {
+      errorCode_ = errno;      
+      printf("ecmcSocketCAN: read() fail with error %s.\n", strerror(errno));      
+      refreshNeeded_ = 1;
+      continue;
+    }
+
+    // forward all data to devices (including master)
+    for(int i = 0; i < deviceCounter_; i++){
+      devices_[i]->newRxFrame(&rxmsg_);
+    }
+
+    if(cfgDbgMode_) {
+      // Simulate candump printout
+      printf("r 0x%03X", rxmsg_.can_id);
+      printf(" [%d]", rxmsg_.can_dlc);
+      for(int i=0; i<rxmsg_.can_dlc; i++ ) {
+        printf(" 0x%02X", rxmsg_.data[i]);
+      }
+      printf("\n");
+    }
+  }
+}
+
+// Connect socket worker
+void ecmcSocketCAN::doConnectWorker() {
+
+  while(true) {
+    
+    if(destructs_) {
+      return;
+    }
+    doConnectEvent_.wait();
+    if(destructs_) {
+      return;
+    }
+    connectPrivate();
+  }
+}
+
+int ecmcSocketCAN::getlastWritesError() {
+  if(!writeBuffer_) { 
+    return ECMC_CAN_ERROR_WRITE_BUFFER_NULL;
+  }
+  return writeBuffer_->getlastWritesErrorAndReset();
+}
+
+int ecmcSocketCAN::addWriteCAN(uint32_t canId,
+                               uint8_t len,
+                               uint8_t data0,
+                               uint8_t data1,
+                               uint8_t data2,
+                               uint8_t data3,
+                               uint8_t data4,
+                               uint8_t data5,
+                               uint8_t data6,
+                               uint8_t data7) {
+
+  if(!writeBuffer_) { 
+    return ECMC_CAN_ERROR_WRITE_BUFFER_NULL;
+  }
+
+  writeBuffer_->addWriteCAN(canId,
+                            len,
+                            data0,
+                            data1,
+                            data2,
+                            data3,
+                            data4,
+                            data5,
+                            data6,
+                            data7);
+  return 0;
+}
+  
+void  ecmcSocketCAN::execute() {
+
+  for(int i = 0; i < deviceCounter_; i++){
+    devices_[i]->execute();
+  }
+
+  int writeError=getlastWritesError();
+  if (writeError) {
+    errorCode_ = writeError;
+    refreshNeeded_ = 1;
+  }
+  refreshAsynParams();
+  return;
+}
+
+// Avoid issues with std:to_string()
+std::string ecmcSocketCAN::to_string(int value) {
+  std::ostringstream os;
+  os << value;
+  return os.str();
+}
+
+void ecmcSocketCAN::addMaster(uint32_t nodeId,
+                              const char* name,
+                              int lssSampleTimeMs,
+                              int syncSampleTimeMs,
+                              int heartSampleTimeMs) {
+
+  if(masterDev_) {
+   throw std::runtime_error("Master already added.");
+  }
+  if(deviceCounter_ >= ECMC_CAN_MAX_DEVICES) {
+    throw std::out_of_range("Device array full.");
+  }
+  if(nodeId >= 128) {
+    throw std::out_of_range("Node id out of range.");
+  }
+
+  if(lssSampleTimeMs <= 0) {
+    throw std::out_of_range("LSS sample time ms out of range.");
+  }
+
+  if(syncSampleTimeMs <= 0) {
+    throw std::out_of_range("Sync sample time ms out of range.");
+  }
+
+  if(heartSampleTimeMs <= 0) {
+    throw std::out_of_range("Heart sample time ms out of range.");
+  }
+
+  masterDev_ = new ecmcCANOpenMaster(writeBuffer_,
+                                     nodeId,
+                                     exeSampleTimeMs_,
+                                     lssSampleTimeMs,
+                                     syncSampleTimeMs,
+                                     heartSampleTimeMs,
+                                     name,
+                                     cfgDbgMode_);
+  // add as a normal device also for execute and rxframe
+  devices_[deviceCounter_] = masterDev_;
+  deviceCounter_++;
+}
+
+void ecmcSocketCAN::addDevice(uint32_t nodeId,
+                              const char* name,
+                              int heartTimeoutMs){
+  if(deviceCounter_ >= ECMC_CAN_MAX_DEVICES) {
+    throw std::out_of_range("Device array full.");
+  }
+  if(nodeId >= 128) {
+    throw std::out_of_range("Node id out of range.");
+  }
+
+  devices_[deviceCounter_] = new ecmcCANOpenDevice(writeBuffer_,nodeId,exeSampleTimeMs_,name,heartTimeoutMs,cfgDbgMode_);  
+  deviceCounter_++;
+}
+
+int ecmcSocketCAN::findDeviceWithNodeId(uint32_t nodeId) {
+  for(int i=0; i < deviceCounter_;i++) {
+    if(devices_[i]) {
+      if(devices_[i]->getNodeId() == nodeId) {
+         return i;
+      }
+    }
+  }
+  return -1;
+}
+
+void ecmcSocketCAN::addPDO(uint32_t nodeId,
+                           uint32_t cobId,
+                           ecmc_can_direction rw,
+                           uint32_t ODSize,
+                           int readTimeoutMs,
+                           int writeCycleMs,    //if <0 then write on demand.
+                           const char* name) {
+  int devId = findDeviceWithNodeId(nodeId);
+  if(devId < 0) {
+    throw std::out_of_range("Node id not found in any configured device.");
+  }
+
+  int errorCode = devices_[devId]->addPDO(cobId,
+                                          rw,
+                                          ODSize,
+                                          readTimeoutMs,
+                                          writeCycleMs,
+                                          name);
+  if(errorCode > 0) {
+    throw std::runtime_error("AddPDO() failed.");
+  }
+}
+              
+void ecmcSocketCAN::addSDO(uint32_t nodeId,
+                           uint32_t cobIdTx,    // 0x580 + CobId
+                           uint32_t cobIdRx,    // 0x600 + Cobid
+                           ecmc_can_direction rw,
+                           uint16_t ODIndex,    // Object dictionary index
+                           uint8_t ODSubIndex,  // Object dictionary subindex
+                           uint32_t ODSize,
+                           int readSampleTimeMs,
+                           const char* name) {
+
+  int devId = findDeviceWithNodeId(nodeId);
+  if(devId < 0) {
+    throw std::out_of_range("Node id not found in any configured device.");
+  }
+
+  int errorCode = devices_[devId]->addSDO(cobIdTx,
+                                          cobIdRx,
+                                          rw,
+                                          ODIndex,
+                                          ODSubIndex,
+                                          ODSize,
+                                          readSampleTimeMs,
+                                          name);
+  if(errorCode > 0) {
+    throw std::runtime_error("AddSDO() failed.");
+  }
+}
+
+void ecmcSocketCAN::initAsyn() {
+
+  ecmcAsynPortDriver *ecmcAsynPort = (ecmcAsynPortDriver *)getEcmcAsynPortDriver();
+  if(!ecmcAsynPort) {
+    printf("ERROR: ecmcAsynPort NULL.");
+    throw std::runtime_error( "ERROR: ecmcAsynPort NULL." );
+  }
+ 
+  // Add resultdata "plugin.can.read.error"
+  std::string paramName = ECMC_PLUGIN_ASYN_PREFIX + std::string(".read.error");
+
+  errorParam_ = ecmcAsynPort->addNewAvailParam(
+                                          paramName.c_str(),     // name
+                                          asynParamInt32,        // asyn type 
+                                          (uint8_t*)&errorCode_, // pointer to data
+                                          sizeof(errorCode_),    // size of data
+                                          ECMC_EC_U32,           // ecmc data type
+                                          0);                    // die if fail
+
+  if(!errorParam_) {
+    printf("ERROR: Failed create asyn param for data.");
+    throw std::runtime_error( "ERROR: Failed create asyn param for: " + paramName);
+  }
+  errorParam_->setAllowWriteToEcmc(false);  // need to callback here
+  errorParam_->refreshParam(1); // read once into asyn param lib
+  ecmcAsynPort->callParamCallbacks(ECMC_ASYN_DEFAULT_LIST, ECMC_ASYN_DEFAULT_ADDR);
+
+  // Add resultdata "plugin.can.read.connected"
+  paramName = ECMC_PLUGIN_ASYN_PREFIX + std::string(".read.connected");
+
+  connectedParam_ = ecmcAsynPort->addNewAvailParam(
+                                          paramName.c_str(),     // name
+                                          asynParamInt32,        // asyn type 
+                                          (uint8_t*)&connected_, // pointer to data
+                                          sizeof(connected_),    // size of data
+                                          ECMC_EC_U32,           // ecmc data type
+                                          0);                    // die if fail
+
+  if(!connectedParam_) {
+    printf("ERROR: Failed create asyn param for connected.");
+    throw std::runtime_error( "ERROR: Failed create asyn param for: " + paramName);
+  }
+  connectedParam_->setAllowWriteToEcmc(false);  // need to callback here
+  connectedParam_->refreshParam(1); // read once into asyn param lib
+  ecmcAsynPort->callParamCallbacks(ECMC_ASYN_DEFAULT_LIST, ECMC_ASYN_DEFAULT_ADDR); 
+}
+
+// only refresh from "execute" thread
+void ecmcSocketCAN::refreshAsynParams() {
+  if(refreshNeeded_) {
+    connectedParam_->refreshParamRT(1); // read once into asyn param lib
+    errorParam_->refreshParamRT(1); // read once into asyn param lib
+  }
+  refreshNeeded_ = 0;
+}
\ No newline at end of file
diff --git a/ecmc_plugin_grbl/ecmcGrbl.h b/ecmc_plugin_grbl/ecmcGrbl.h
new file mode 100644
index 0000000..b85be95
--- /dev/null
+++ b/ecmc_plugin_grbl/ecmcGrbl.h
@@ -0,0 +1,148 @@
+/*************************************************************************\
+* Copyright (c) 2019 European Spallation Source ERIC
+* ecmc is distributed subject to a Software License Agreement found
+* in file LICENSE that is included with this distribution. 
+*
+*  ecmcGrbl.h
+*
+*  Created on: jan 12, 2022
+*      Author: anderssandstrom
+*
+\*************************************************************************/
+#ifndef ECMC_GRBL_H_
+#define ECMC_GRBL_H_
+
+#include <stdexcept>
+#include "ecmcDataItem.h"
+#include "ecmcAsynPortDriver.h"
+#include "ecmcSocketCANDefs.h"
+#include "ecmcSocketCANWriteBuffer.h"
+#include "ecmcCANOpenDevice.h"
+#include "ecmcCANOpenMaster.h"
+#include "inttypes.h"
+#include <string>
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+
+#include <net/if.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/ioctl.h>
+
+#include <linux/can.h>
+#include <linux/can/raw.h>
+
+#define ECMC_CAN_MAX_WRITE_CMDS 128
+#define ECMC_CAN_MAX_DEVICES 128
+#define ECMC_CAN_ERROR_WRITE_FULL 10
+#define ECMC_CAN_ERROR_WRITE_BUSY 11
+#define ECMC_CAN_ERROR_WRITE_NO_DATA 12
+#define ECMC_CAN_ERROR_WRITE_INCOMPLETE 13
+#define ECMC_CAN_ERROR_WRITE_BUFFER_NULL 14
+
+class ecmcSocketCAN {
+ public:
+
+  /** ecmc ecmcSocketCAN class
+   * This object can throw: 
+   *    - bad_alloc
+   *    - invalid_argument
+   *    - runtime_error
+   *    - out_of_range
+  */
+  ecmcSocketCAN(char* configStr,
+                char* portName,
+                int exeSampelTimeMs);
+  ~ecmcSocketCAN();
+
+  void doReadWorker();
+  void doWriteWorker();
+  void doConnectWorker();
+
+  //virtual asynStatus    writeInt32(asynUser *pasynUser, epicsInt32 value);
+  //virtual asynStatus    readInt32(asynUser *pasynUser, epicsInt32 *value);
+  //virtual asynStatus    readInt8Array(asynUser *pasynUser, epicsInt8 *value, 
+  //                                    size_t nElements, size_t *nIn);
+  //virtual asynStatus    readFloat64(asynUser *pasynUser, epicsFloat64 *value);
+  void                  connectExternal();  
+  int                   getConnected();
+  int                   addWriteCAN(uint32_t canId,
+                                    uint8_t len,
+                                    uint8_t data0,
+                                    uint8_t data1,
+                                    uint8_t data2,
+                                    uint8_t data3,
+                                    uint8_t data4,
+                                    uint8_t data5,
+                                    uint8_t data6,
+                                    uint8_t data7);
+  int                   getlastWritesError();
+  void                  execute();  // ecmc rt loop
+  
+  void                  addMaster(uint32_t nodeId,
+                                  const char* name,
+                                  int lssSampleTimeMs,
+                                  int syncSampleTimeMs,
+                                  int heartSampleTimeMs);
+
+  void                  addDevice(uint32_t nodeId,
+                                  const char* name,
+                                  int heartTimeoutMs);
+
+  void                  addPDO(uint32_t nodeId,
+                               uint32_t cobId,
+                               ecmc_can_direction rw,
+                               uint32_t ODSize,
+                               int readTimeoutMs,
+                               int writeCycleMs,    //if <0 then write on demand.
+                               const char* name);
+                  
+  void                  addSDO(uint32_t nodeId,
+                               uint32_t cobIdTx,    // 0x580 + CobId
+                               uint32_t cobIdRx,    // 0x600 + Cobid
+                               ecmc_can_direction rw,
+                               uint16_t ODIndex,    // Object dictionary index
+                               uint8_t ODSubIndex,  // Object dictionary subindex
+                               uint32_t ODSize,
+                               int readSampleTimeMs,
+                               const char* name);
+
+  int                   findDeviceWithNodeId(uint32_t nodeId);
+
+ private:
+  void                  parseConfigStr(char *configStr);
+  static std::string    to_string(int value);
+  void                  connectPrivate();
+  int                   writeCAN(can_frame *frame);
+  char*                 cfgCanIFStr_;   // Config: can interface can0, vcan0..
+  int                   cfgDbgMode_;
+  int                   cfgAutoConnect_;
+  int                   destructs_;
+  int                   connected_;
+  epicsEvent            doConnectEvent_;
+  epicsEvent            doWriteEvent_;
+  struct can_frame      rxmsg_;
+  struct ifreq          ifr_;
+  int                   socketId_;
+  struct sockaddr_can   addr_;
+  struct can_frame      txmsgBuffer_[ECMC_CAN_MAX_WRITE_CMDS];
+  int                   exeSampleTimeMs_;
+  ecmcSocketCANWriteBuffer *writeBuffer_;
+
+  int                deviceCounter_; 
+  ecmcCANOpenDevice *devices_[ECMC_CAN_MAX_DEVICES];
+  ecmcCANOpenMaster *masterDev_;
+
+  int                errorCode_;
+  int                refreshNeeded_;
+  //ASYN
+  void initAsyn();
+  void refreshAsynParams();
+  ecmcAsynDataItem *errorParam_;
+  ecmcAsynDataItem *connectedParam_;
+};
+
+#endif  /* ECMC_GRBL_H_ */
diff --git a/ecmc_plugin_grbl/ecmcGrblDefs.h b/ecmc_plugin_grbl/ecmcGrblDefs.h
new file mode 100644
index 0000000..c1f3f6e
--- /dev/null
+++ b/ecmc_plugin_grbl/ecmcGrblDefs.h
@@ -0,0 +1,38 @@
+/*************************************************************************\
+* Copyright (c) 2019 European Spallation Source ERIC
+* ecmc is distributed subject to a Software License Agreement found
+* in file LICENSE that is included with this distribution. 
+*
+*  ecmcGrblDefs.h
+*
+*  Created on: Jan 12, 2022
+*      Author: anderssandstrom
+*
+\*************************************************************************/
+
+#ifndef ECMC_GRBL_DEFS_H_
+#define ECMC_GRBL_DEFS_H_
+
+#include "grbl.h"
+
+// Options
+#define ECMC_PLUGIN_DBG_PRINT_OPTION_CMD   "DBG_PRINT="
+#define ECMC_PLUGIN_ASYN_PREFIX            "plugin.grbl"
+
+
+system_t sys;
+int32_t sys_position[N_AXIS];      // Real-time machine (aka home) position vector in steps.
+int32_t sys_probe_position[N_AXIS]; // Last probe position in machine coordinates and steps.
+volatile uint8_t sys_probe_state;   // Probing state value.  Used to coordinate the probing cycle with stepper ISR.
+volatile uint8_t sys_rt_exec_state;   // Global realtime executor bitflag variable for state management. See EXEC bitmasks.
+volatile uint8_t sys_rt_exec_alarm;   // Global realtime executor bitflag variable for setting various alarms.
+volatile uint8_t sys_rt_exec_motion_override; // Global realtime executor bitflag variable for motion-based overrides.
+volatile uint8_t sys_rt_exec_accessory_override; // Global realtime executor bitflag variable for spindle/coolant overrides.
+#ifdef DEBUG
+  volatile uint8_t sys_rt_exec_debug;
+#endif
+
+
+
+
+#endif  /* ECMC_GRBL_DEFS_H_ */
diff --git a/ecmc_plugin_grbl/ecmcGrblWrap.cpp b/ecmc_plugin_grbl/ecmcGrblWrap.cpp
new file mode 100644
index 0000000..c17f05c
--- /dev/null
+++ b/ecmc_plugin_grbl/ecmcGrblWrap.cpp
@@ -0,0 +1,560 @@
+/*************************************************************************\
+* Copyright (c) 2019 European Spallation Source ERIC
+* ecmc is distributed subject to a Software License Agreement found
+* in file LICENSE that is included with this distribution. 
+*
+*  ecmcFFTWrap.cpp
+*
+*  Created on: Mar 22, 2020
+*      Author: anderssandstrom
+*
+\*************************************************************************/
+
+// Needed to get headers in ecmc right...
+#define ECMC_IS_PLUGIN
+
+#include <vector>
+#include <stdexcept>
+#include <string>
+#include "ecmcSocketCANWrap.h"
+#include "ecmcSocketCAN.h"
+#include "ecmcSocketCANDefs.h"
+#include <epicsTypes.h>
+#include <epicsTime.h>
+#include <epicsThread.h>
+#include <epicsString.h>
+#include <epicsTimer.h>
+#include <epicsMutex.h>
+#include <epicsExport.h>
+#include <epicsEvent.h>
+
+#include <iocsh.h>
+
+
+#define ECMC_PLUGIN_MAX_PORTNAME_CHARS 64
+#define ECMC_PLUGIN_PORTNAME_PREFIX "PLUGIN.CAN"
+
+static ecmcSocketCAN*  can = NULL;
+static char            portNameBuffer[ECMC_PLUGIN_MAX_PORTNAME_CHARS];
+
+int createSocketCAN(char* configStr, int exeSampleTimeMs) {
+
+  // create new ecmcFFT object
+
+  // create asynport name for new object ()
+  memset(portNameBuffer, 0, ECMC_PLUGIN_MAX_PORTNAME_CHARS);
+  snprintf (portNameBuffer, ECMC_PLUGIN_MAX_PORTNAME_CHARS,
+            ECMC_PLUGIN_PORTNAME_PREFIX);
+  try {
+    can = new ecmcSocketCAN(configStr, portNameBuffer, exeSampleTimeMs);
+  }
+  catch(std::exception& e) {
+    if(can) {
+      delete can;
+    }
+    printf("Exception: %s. Plugin will unload.\n",e.what());
+    return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE;
+  }
+  
+  return 0;
+}
+
+int connectSocketCAN() {
+  if(can){
+      try {       
+        can->connectExternal();
+    }
+    catch(std::exception& e) {
+      printf("Exception: %s.\n",e.what());
+      return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE;
+    }
+  }
+  else {
+    return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE;
+  }
+  return 0;
+}
+
+int getSocketCANConnectd() {
+  if(can){
+    try {       
+      return can->getConnected();      
+    }
+    catch(std::exception& e) {
+      printf("Exception: %s.\n",e.what());
+      return 0;
+    }
+  }
+  return 0;
+}
+
+int getlastWritesError() {
+  if(can){
+    try {       
+      return can->getlastWritesError();
+    }
+    catch(std::exception& e) {
+      printf("Exception: %s.\n",e.what());
+      return 1;
+    }
+  }
+  return 1;
+}
+
+int execute() {
+  if(can){
+    can->execute();
+  }
+  return 0;
+}
+
+int addWriteSocketCAN( double canId,
+                       double len,
+                       double data0,
+                       double data1,
+                       double data2,
+                       double data3,
+                       double data4,
+                       double data5,
+                       double data6,
+                       double data7) {
+  if(can){
+    try {       
+      return can->addWriteCAN((uint32_t) canId,
+                              (uint8_t) len,
+                              (uint8_t) data0,
+                              (uint8_t) data1,
+                              (uint8_t) data2,
+                              (uint8_t) data3,
+                              (uint8_t) data4,
+                              (uint8_t) data5,
+                              (uint8_t) data6,
+                              (uint8_t) data7);      
+    }
+    catch(std::exception& e) {
+      printf("Exception: %s.\n",e.what());
+      return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE;
+    }
+  }
+  return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE;
+}
+
+void deleteSocketCAN() {
+  if(can) {
+    delete (can);
+  }
+}
+
+
+/** 
+ * EPICS iocsh shell command: ecmcCANOpenAddMaster
+*/
+
+void ecmcCANOpenAddMasterPrintHelp() {
+  printf("\n");
+  printf("       Use ecmcCANOpenAddMaster(<name>, <node id>,....)\n");
+  printf("          <name>                         : Name of master device.\n");
+  printf("          <node id>                      : CANOpen node id of master.\n");
+  printf("          <LSS sample time ms>           : Sample time for LSS.\n");
+  printf("          <Sync sample time ms>          : Sample time for SYNC.\n");
+  printf("          <NMT Heartbeat sample time ms> : Sample time for NMT Heartbeat.\n");
+  printf("\n");
+}
+
+int ecmcCANOpenAddMaster(const char* name,
+                         int nodeId,
+                         int lssSampleTimeMs,
+                         int syncSampleTimeMs,
+                         int heartSampleTimeMs) {
+
+  if(!name) {
+    printf("Error: name.\n");
+    ecmcCANOpenAddMasterPrintHelp();
+    return asynError;
+  }
+
+  if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) {
+    ecmcCANOpenAddMasterPrintHelp();
+    return asynSuccess;
+  }
+
+  if(!can) {
+    printf("Plugin not initialized/loaded.\n");
+    return asynError;
+  }
+
+  try {
+    can->addMaster((uint32_t)nodeId,
+                   name,
+                   lssSampleTimeMs,
+                   syncSampleTimeMs,
+                   heartSampleTimeMs);
+  }
+  catch(std::exception& e) {
+    printf("Exception: %s. Add master failed.\n",e.what());
+    return asynError;
+  }
+  
+  return asynSuccess;
+}
+
+static const iocshArg initArg0_0 =
+{ "Name", iocshArgString };
+static const iocshArg initArg1_0 =
+{ "Node Id", iocshArgInt };
+static const iocshArg initArg2_0 =
+{ "LSS sample time ms", iocshArgInt };
+static const iocshArg initArg3_0 =
+{ "Sync sample time ms", iocshArgInt };
+static const iocshArg initArg4_0 =
+{ "NMT Heart sample time ms", iocshArgInt };
+
+static const iocshArg *const initArgs_0[]  = { &initArg0_0, 
+                                               &initArg1_0,
+                                               &initArg2_0,
+                                               &initArg3_0,
+                                               &initArg4_0};
+
+static const iocshFuncDef    initFuncDef_0 = { "ecmcCANOpenAddMaster", 5, initArgs_0 };
+static void initCallFunc_0(const iocshArgBuf *args) {
+  ecmcCANOpenAddMaster(args[0].sval,
+                       args[1].ival,
+                       args[2].ival,
+                       args[3].ival,
+                       args[4].ival);
+}
+
+/** 
+ * EPICS iocsh shell command: ecmcCANOpenAddDevice
+*/
+
+void ecmcCANOpenAddDevicePrintHelp() {
+  printf("\n");
+  printf("       Use ecmcCANOpenAddDevice(<name>, <node id>)\n");
+  printf("          <name>                     : Name of device.\n");
+  printf("          <node id>                  : CANOpen node id of device.\n");
+  printf("          <NMT Heartbeat timeout ms> : Timeout for NMT Heartbeat.\n");
+  printf("\n");
+}
+
+int ecmcCANOpenAddDevice(const char* name, int nodeId,int heartTimeOutMs) {
+  if(!name) {
+    printf("Error: name.\n");
+    ecmcCANOpenAddDevicePrintHelp();
+    return asynError;
+  }
+
+  if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) {
+    ecmcCANOpenAddDevicePrintHelp();
+    return asynSuccess;
+  }
+
+  if(!can) {
+    printf("Plugin not initialized/loaded.\n");
+    return asynError;
+  }
+
+  if(heartTimeOutMs < 0) {
+    printf("Invalid NMT heartbeat timeout.\n");
+    return asynError;
+  }
+
+  try {
+    can->addDevice((uint32_t)nodeId,name,heartTimeOutMs);
+  }
+  catch(std::exception& e) {
+    printf("Exception: %s. Add device failed.\n",e.what());
+    return asynError;
+  }
+
+  return asynSuccess;
+}
+
+static const iocshArg initArg0_1 =
+{ "Name", iocshArgString };
+static const iocshArg initArg1_1 =
+{ "Node Id", iocshArgInt };
+static const iocshArg initArg2_1 =
+{ "NMT Heart timeout ms", iocshArgInt };
+
+static const iocshArg *const initArgs_1[]  = { &initArg0_1, 
+                                               &initArg1_1,
+                                               &initArg2_1};
+
+static const iocshFuncDef    initFuncDef_1 = { "ecmcCANOpenAddDevice", 3, initArgs_1 };
+static void initCallFunc_1(const iocshArgBuf *args) {
+  ecmcCANOpenAddDevice(args[0].sval, args[1].ival, args[2].ival);
+}
+
+/** 
+ * EPICS iocsh shell command: ecmcCANOpenAddSDO
+*/
+
+void ecmcCANOpenAddSDOPrintHelp() {
+  printf("\n");
+  printf("  Use ecmcCANOpenAddSDO(<name>, <node id>,.....)\n");
+  printf("          <name>            : Name of master device.\n");
+  printf("          <node id>         : CANOpen node id of device/master.\n");
+  printf("          <cob id tx>       : CANOpen cob id of Tx of slave SDO.\n");
+  printf("          <cob id rx>       : CANOpen cob id of Rx of slave SDO.\n");
+  printf("          <dir>             : Direction 1=write and 2=read.\n");
+  printf("          <ODIndex>         : OD index of SDO.\n");
+  printf("          <ODSubIndex>      : OD sub index of SDO.\n");
+  printf("          <ODSize>          : OS Size.\n");
+  printf("          <readSampleTimeMs>: Sample time for read in ms (write is always on demand).\n");
+  printf("\n");
+}
+
+int ecmcCANOpenAddSDO(const char* name, 
+                      int nodeId, 
+                      int cobIdTx,
+                      int cobIdRx, 
+                      int dir,
+                      int ODIndex, 
+                      int ODSubIndex,
+                      int ODSize,
+                      int readSampleTimeMs) {
+  if(!name) {
+    printf("Error: name.\n");
+    ecmcCANOpenAddSDOPrintHelp();
+    return asynError;
+  }
+
+  if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) {
+    ecmcCANOpenAddSDOPrintHelp();
+    return asynSuccess;
+  }
+
+  if(cobIdRx < 0) {
+    printf("Error: invalid cobIdRx.\n");
+    ecmcCANOpenAddSDOPrintHelp();
+    return asynError;
+  }
+
+  if(cobIdTx < 0) {
+    printf("Error: invalid cobIdTx.\n");
+    ecmcCANOpenAddSDOPrintHelp();
+    return asynError;
+  }
+
+  if(dir > 2 || dir <= 0) {
+    printf("Error: invalid dir.\n");
+    ecmcCANOpenAddSDOPrintHelp();
+    return asynError;
+  }
+
+  if(ODIndex < 0) {
+    printf("Error: invalid ODIndex.\n");
+    ecmcCANOpenAddSDOPrintHelp();
+    return asynError;
+  }
+
+  if(ODSubIndex < 0) {
+    printf("Error: invalid ODSubIndex.\n");
+    ecmcCANOpenAddSDOPrintHelp();
+    return asynError;
+  }
+
+  if(ODSize < 0) {
+    printf("Error: invalid ODSize.\n");
+    ecmcCANOpenAddSDOPrintHelp();
+    return asynError;
+  }
+
+  if(readSampleTimeMs < 0) {
+    printf("Error: invalid readSampleTimeMs.\n");
+    ecmcCANOpenAddSDOPrintHelp();
+    return asynError;
+  }
+
+  ecmc_can_direction tempDir = DIR_READ;
+  if(dir == 1) {
+    tempDir = DIR_WRITE;
+  }
+
+  try {
+    can->addSDO((uint32_t)nodeId,
+                 cobIdTx,
+                 cobIdRx,
+                 tempDir,
+                 ODIndex,
+                 ODSubIndex,
+                 ODSize,
+                 readSampleTimeMs,
+                 name);
+
+
+  }
+  catch(std::exception& e) {
+    printf("Exception: %s. Add PDO failed.\n",e.what());
+    return asynError;
+  }
+  return asynSuccess;
+}
+
+static const iocshArg initArg0_2 =
+{ "Name", iocshArgString };
+static const iocshArg initArg1_2 =
+{ "Node Id", iocshArgInt };
+static const iocshArg initArg2_2 =
+{ "COB id TX", iocshArgInt };
+static const iocshArg initArg3_2 =
+{ "COB id RX", iocshArgInt };
+static const iocshArg initArg4_2 =
+{ "Direction", iocshArgInt };
+static const iocshArg initArg5_2 =
+{ "OD Index", iocshArgInt };
+static const iocshArg initArg6_2 =
+{ "OD sub index", iocshArgInt };
+static const iocshArg initArg7_2 =
+{ "OD size", iocshArgInt };
+static const iocshArg initArg8_2 =
+{ "Read sample time ms", iocshArgInt };
+
+static const iocshArg *const initArgs_2[]  = { &initArg0_2, 
+                                               &initArg1_2,
+                                               &initArg2_2,
+                                               &initArg3_2,
+                                               &initArg4_2,
+                                               &initArg5_2,
+                                               &initArg6_2,
+                                               &initArg7_2,
+                                               &initArg8_2};
+
+static const iocshFuncDef    initFuncDef_2 = { "ecmcCANOpenAddSDO", 9, initArgs_2 };
+static void initCallFunc_2(const iocshArgBuf *args) {
+  ecmcCANOpenAddSDO(args[0].sval,
+                    args[1].ival,
+                    args[2].ival,
+                    args[3].ival,
+                    args[4].ival,
+                    args[5].ival,
+                    args[6].ival,
+                    args[7].ival,
+                    args[8].ival);
+}
+
+/** 
+ * EPICS iocsh shell command: ecmcCANOpenAddPDO
+*/
+void ecmcCANOpenAddPDOPrintHelp() {
+  printf("\n");
+  printf("     Use \"ecmcCANOpenAddPDO(<name>, <node id>\n");
+  printf("          <name>            : Name of master device.\n");
+  printf("          <node id>         : CANOpen node id of device/master.\n");
+  printf("          <cob id>          : CANOpen cob id of PDO.\n");
+  printf("          <dir>             : Direction 1=write and 2=read.\n");
+  printf("          <ODSize>          : Size of PDO (max 8 bytes).\n");
+  printf("          <readTimeoutMs>   : Readtimeout in ms.\n");
+  printf("          <writeCycleMs>    : Cycle time for write (if <= 0 then only write on change).\n");
+  printf("\n");
+}
+
+int ecmcCANOpenAddPDO(const char* name, 
+                      int nodeId, 
+                      int cobId, 
+                      int dir, 
+                      int ODSize,
+                      int readTimeoutMs,
+                      int writeCycleMs) {
+  if(!name) {
+    printf("Error: name.\n");
+    ecmcCANOpenAddPDOPrintHelp();
+    return asynError;
+  }
+
+  if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) {
+    ecmcCANOpenAddPDOPrintHelp();
+    return asynSuccess;
+  }
+
+  if(dir > 2 || dir <= 0) {
+    printf("Error: invalid dir.\n");
+    ecmcCANOpenAddPDOPrintHelp();
+    return asynError;
+  }
+
+  if(ODSize < 0) {
+    printf("Error: invalid ODSize.\n");
+    ecmcCANOpenAddPDOPrintHelp();
+    return asynError;
+  }
+
+  if(readTimeoutMs < 0) {
+    printf("Error: invalid readTimeoutMs.\n");
+    ecmcCANOpenAddPDOPrintHelp();
+    return asynError;
+  }
+
+  if(writeCycleMs < 0) {
+    printf("Error: invalid writeCycleMs.\n");
+    ecmcCANOpenAddPDOPrintHelp();
+    return asynError;
+  }
+
+  ecmc_can_direction tempDir = DIR_READ;
+  if(dir == 1) {
+    tempDir = DIR_WRITE;
+  }
+
+  try {
+    can->addPDO((uint32_t)nodeId,
+                 cobId,
+                 tempDir,
+                 ODSize,
+                 readTimeoutMs,
+                 writeCycleMs,name);
+  }
+  catch(std::exception& e) {
+    printf("Exception: %s. Add PDO failed.\n",e.what());
+    return asynError;
+  }
+  return asynSuccess;
+}
+
+static const iocshArg initArg0_3 =
+{ "Name", iocshArgString };
+static const iocshArg initArg1_3 =
+{ "Node Id", iocshArgInt };
+static const iocshArg initArg2_3 =
+{ "COB Id", iocshArgInt };
+static const iocshArg initArg3_3 =
+{ "Direction", iocshArgInt };
+static const iocshArg initArg4_3 =
+{ "ODSize", iocshArgInt };
+static const iocshArg initArg5_3 =
+{ "Read Timeout ms", iocshArgInt };
+static const iocshArg initArg6_3 =
+{ "Write cycle ms", iocshArgInt };
+
+static const iocshArg *const initArgs_3[]  = { &initArg0_3,
+                                               &initArg1_3,
+                                               &initArg2_3,
+                                               &initArg3_3,
+                                               &initArg4_3,
+                                               &initArg5_3,
+                                               &initArg6_3
+                                               };
+
+static const iocshFuncDef    initFuncDef_3 = { "ecmcCANOpenAddPDO", 7, initArgs_3 };
+static void initCallFunc_3(const iocshArgBuf *args) {
+  ecmcCANOpenAddPDO(args[0].sval,
+                    args[1].ival,
+                    args[2].ival,
+                    args[3].ival,
+                    args[4].ival,
+                    args[5].ival,
+                    args[6].ival);
+}
+
+/** 
+ * Register all functions
+*/
+void ecmcCANPluginDriverRegister(void) {
+  iocshRegister(&initFuncDef_0,    initCallFunc_0);   // ecmcCANOpenAddMaster
+  iocshRegister(&initFuncDef_1,    initCallFunc_1);   // ecmcCANOpenAddDevice
+  iocshRegister(&initFuncDef_2,    initCallFunc_2);   // ecmcCANOpenAddSDO
+  iocshRegister(&initFuncDef_3,    initCallFunc_3);   // ecmcCANOpenAddPDO
+}
+
+epicsExportRegistrar(ecmcCANPluginDriverRegister);
diff --git a/ecmc_plugin_grbl/ecmcGrblWrap.h b/ecmc_plugin_grbl/ecmcGrblWrap.h
new file mode 100644
index 0000000..678481d
--- /dev/null
+++ b/ecmc_plugin_grbl/ecmcGrblWrap.h
@@ -0,0 +1,72 @@
+/*************************************************************************\
+* Copyright (c) 2019 European Spallation Source ERIC
+* ecmc is distributed subject to a Software License Agreement found
+* in file LICENSE that is included with this distribution. 
+*
+*  ecmcGrblWrap.h
+*
+*  Created on: Jan 12, 2022
+*      Author: anderssandstrom
+*
+\*************************************************************************/
+#ifndef ECMC_GRBL_WRAP_H_
+#define ECMC_GRBL_WRAP_H_
+#include "ecmcSocketCANDefs.h"
+
+# ifdef __cplusplus
+extern "C" {
+# endif  // ifdef __cplusplus
+
+/** \brief Create new SocketCAN object
+ *
+ *  The configuration string needs to define tha can interface by:\n
+ *  "IF=<data source>;"\n
+ *  Example:\n
+ *  "IF=can0";\n
+ *  \param[in] configStr Configuration string.\n
+ *
+ *  \return 0 if success or otherwise an error code.\n
+ */
+int createSocketCAN(char *configStr, int exeSampleTimeMs);
+
+/** \brief Connect to SocketCAN interface\n
+ */
+
+int connectSocketCAN();
+
+/** \brief Connected to can interface\n
+ */
+int getSocketCANConnectd();
+
+/** \brief Get last error from writes\n
+  */
+int getlastWritesError();
+
+/** \brief execute from rt loop\n
+  */
+int execute();
+
+/** \brief add CAN frame to write buffer
+ */
+int  addWriteSocketCAN( double canId,
+                        double len,
+                        double data0,
+                        double data1,
+                        double data2,
+                        double data3,
+                        double data4,
+                        double data5,
+                        double data6,
+                        double data7);
+
+/** \brief Delete SocketCAN object\n
+ *
+ * Should be called when destructs.\n
+ */
+void deleteSocketCAN();
+
+# ifdef __cplusplus
+}
+# endif  // ifdef __cplusplus
+
+#endif  /* ECMC_GRBL_WRAP_H_ */
diff --git a/ecmc_plugin_grbl/ecmcPluginGrbl.c b/ecmc_plugin_grbl/ecmcPluginGrbl.c
new file mode 100644
index 0000000..bc7d338
--- /dev/null
+++ b/ecmc_plugin_grbl/ecmcPluginGrbl.c
@@ -0,0 +1,221 @@
+/*************************************************************************\
+* Copyright (c) 2019 European Spallation Source ERIC
+* ecmc is distributed subject to a Software License Agreement found
+* in file LICENSE that is included with this distribution. 
+*
+*  ecmcPluginExample.cpp
+*
+*  Created on: Mar 21, 2020
+*      Author: anderssandstrom
+*      Credits to  https://github.com/sgreg/dynamic-loading 
+*
+\*************************************************************************/
+
+// Needed to get headers in ecmc right...
+#define ECMC_IS_PLUGIN
+#define ECMC_EXAMPLE_PLUGIN_VERSION 2
+
+#ifdef __cplusplus
+extern "C" {
+#endif  // ifdef __cplusplus
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "ecmcPluginDefs.h"
+#include "ecmcPluginClient.h"
+#include "ecmcGrblDefs.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+
+static int    lastEcmcError   = 0;
+static char*  lastConfStr     = NULL;
+static int    alreadyLoaded   = 0;
+
+/** Optional. 
+ *  Will be called once after successfull load into ecmc.
+ *  Return value other than 0 will be considered error.
+ *  configStr can be used for configuration parameters.
+ **/
+int grblConstruct(char *configStr)
+{
+  if(alreadyLoaded) {    
+    return 1;
+  }
+
+  alreadyLoaded = 1;
+  // create SocketCAN object and register data callback
+  lastConfStr = strdup(configStr);
+
+  // Initialize system upon power-up.
+  serial_init();   // Setup serial baud rate and interrupts
+  settings_init(); // Load Grbl settings from EEPROM
+  stepper_init();  // Configure stepper pins and interrupt timers
+  system_init();   // Configure pinout pins and pin-change interrupt
+
+  memset(sys_position,0,sizeof(sys_position)); // Clear machine position.
+  //sei(); // Enable interrupts
+
+  // Initialize system state.
+  #ifdef FORCE_INITIALIZATION_ALARM
+    // Force Grbl into an ALARM state upon a power-cycle or hard reset.
+    sys.state = STATE_ALARM;
+  #else
+    sys.state = STATE_IDLE;
+  #endif
+  
+  // Check for power-up and set system alarm if homing is enabled to force homing cycle
+  // by setting Grbl's alarm state. Alarm locks out all g-code commands, including the
+  // startup scripts, but allows access to settings and internal commands. Only a homing
+  // cycle '$H' or kill alarm locks '$X' will disable the alarm.
+  // NOTE: The startup script will run after successful completion of the homing cycle, but
+  // not after disabling the alarm locks. Prevents motion startup blocks from crashing into
+  // things uncontrollably. Very bad.
+  #ifdef HOMING_INIT_LOCK
+    if (bit_istrue(settings.flags,BITFLAG_HOMING_ENABLE)) { sys.state = STATE_ALARM; }
+  #endif
+
+  // Grbl initialization loop upon power-up or a system abort. For the latter, all processes
+  // will return to this loop to be cleanly re-initialized.
+  //for(;;) {
+
+    // Reset system variables.
+    uint8_t prior_state = sys.state;
+    memset(&sys, 0, sizeof(system_t)); // Clear system struct variable.
+    sys.state = prior_state;
+    sys.f_override = DEFAULT_FEED_OVERRIDE;  // Set to 100%
+    sys.r_override = DEFAULT_RAPID_OVERRIDE; // Set to 100%
+    sys.spindle_speed_ovr = DEFAULT_SPINDLE_SPEED_OVERRIDE; // Set to 100%
+		memset(sys_probe_position,0,sizeof(sys_probe_position)); // Clear probe position.
+    sys_probe_state = 0;
+    sys_rt_exec_state = 0;
+    sys_rt_exec_alarm = 0;
+    sys_rt_exec_motion_override = 0;
+    sys_rt_exec_accessory_override = 0;
+
+    // Reset Grbl primary systems.
+    serial_reset_read_buffer(); // Clear serial read buffer
+    gc_init(); // Set g-code parser to default state
+    spindle_init();
+    coolant_init();
+    limits_init();
+    probe_init();
+    plan_reset(); // Clear block buffer and planner variables
+    st_reset(); // Clear stepper subsystem variables.
+
+    // Sync cleared gcode and planner positions to current system position.
+    plan_sync_position();
+    gc_sync_position();
+
+    // Print welcome message. Indicates an initialization has occured at power-up or with a reset.
+    report_init_message();
+
+    // Start Grbl main loop. Processes program inputs and executes them.
+    protocol_main_loop();
+ //}
+
+  return 0; //createSocketCAN(configStr,getEcmcSampleTimeMS());
+}
+
+/** Optional function.
+ *  Will be called once at unload.
+ **/
+void grblDestruct(void)
+{  
+  if(lastConfStr){
+    free(lastConfStr);
+  }
+  //deleteSocketCAN();
+}
+
+/** Optional function.
+ *  Will be called each realtime cycle if definded
+ *  ecmcError: Error code of ecmc. Makes it posible for 
+ *  this plugin to react on ecmc errors
+ *  Return value other than 0 will be considered to be an error code in ecmc.
+ **/
+int grblRealtime(int ecmcError)
+{  
+  lastEcmcError = ecmcError;
+  return 0; //execute();
+}
+
+/** Link to data source here since all sources should be availabe at this stage
+ *  (for example ecmc PLC variables are defined only at enter of realtime)
+ **/
+int grblEnterRT(){
+  return 0;
+}
+
+/** Optional function.
+ *  Will be called once just before leaving realtime mode
+ *  Return value other than 0 will be considered error.
+ **/
+int grblExitRT(void){
+  return 0;
+}
+// Plc function for connect to can
+double grbl_connect() {
+  return 0;
+}
+
+// Register data for plugin so ecmc know what to use
+struct ecmcPluginData pluginDataDef = {
+  // Allways use ECMC_PLUG_VERSION_MAGIC
+  .ifVersion = ECMC_PLUG_VERSION_MAGIC, 
+  // Name 
+  .name = "ecmcPluginGrbl",
+  // Description
+  .desc = "grbl plugin for use with ecmc.",
+  // Option description
+  .optionDesc = "\n    "ECMC_PLUGIN_DBG_PRINT_OPTION_CMD"<1/0>    : Enables/disables printouts from plugin, default = disabled (=0).\n",
+  // Plugin version
+  .version = ECMC_EXAMPLE_PLUGIN_VERSION,
+  // Optional construct func, called once at load. NULL if not definded.
+  .constructFnc = grblConstruct,
+  // Optional destruct func, called once at unload. NULL if not definded.
+  .destructFnc = grblDestruct,
+  // Optional func that will be called each rt cycle. NULL if not definded.
+  .realtimeFnc = grblRealtime,
+  // Optional func that will be called once just before enter realtime mode
+  .realtimeEnterFnc = grblEnterRT,
+  // Optional func that will be called once just before exit realtime mode
+  .realtimeExitFnc = grblExitRT,
+  // PLC funcs
+  .funcs[0] =
+      { /*----can_connect----*/
+        // Function name (this is the name you use in ecmc plc-code)
+        .funcName = "grbl_connect",
+        // Function description
+        .funcDesc = "double grbl_connect() : Connect to grbl interface (from config str).",
+        /**
+        * 7 different prototypes allowed (only doubles since reg in plc).
+        * Only funcArg${argCount} func shall be assigned the rest set to NULL.
+        **/
+        .funcArg0 = grbl_connect,
+        .funcArg1 = NULL,
+        .funcArg2 = NULL,
+        .funcArg3 = NULL,
+        .funcArg4 = NULL,
+        .funcArg5 = NULL,
+        .funcArg6 = NULL,
+        .funcArg7 = NULL,
+        .funcArg8 = NULL,
+        .funcArg9 = NULL,
+        .funcArg10 = NULL,
+        .funcGenericObj = NULL,
+      },
+  .funcs[1] = {0},  // last element set all to zero..
+  // PLC consts
+  .consts[0] = {0}, // last element set all to zero..
+};
+
+ecmc_plugin_register(pluginDataDef);
+
+# ifdef __cplusplus
+}
+# endif  // ifdef __cplusplus
diff --git a/grbl/grbl.h b/grbl/grbl.h
index 309898c..9b4a325 100644
--- a/grbl/grbl.h
+++ b/grbl/grbl.h
@@ -57,7 +57,7 @@
 #include "probe.h"
 #include "protocol.h"
 #include "report.h"
-//#include "serial.h"
+#include "serial.h"
 #include "spindle_control.h"
 #include "stepper.h"
 #include "jog.h"
diff --git a/grbl/report.c b/grbl/report.c
index 000836f..b113a63 100644
--- a/grbl/report.c
+++ b/grbl/report.c
@@ -31,10 +31,10 @@
 
 // Internal report utilities to reduce flash with repetitive tasks turned into functions.
 void report_util_setting_prefix(uint8_t n) { serial_write('$'); print_uint8_base10(n); serial_write('='); }
-static void report_util_line_feed() { printPgmString(PSTR("\r\n")); }
+static void report_util_line_feed() { printPgmString(("\r\n")); }
 static void report_util_feedback_line_feed() { serial_write(']'); report_util_line_feed(); }
-static void report_util_gcode_modes_G() { printPgmString(PSTR(" G")); }
-static void report_util_gcode_modes_M() { printPgmString(PSTR(" M")); }
+static void report_util_gcode_modes_G() { printPgmString((" G")); }
+static void report_util_gcode_modes_M() { printPgmString((" M")); }
 // static void report_util_comment_line_feed() { serial_write(')'); report_util_line_feed(); }
 static void report_util_axis_values(float *axis_value) {
   uint8_t idx;
@@ -49,28 +49,28 @@ static void report_util_setting_string(uint8_t n) {
   serial_write(' ');
   serial_write('(');
   switch(n) {
-    case 0: printPgmString(PSTR("stp pulse")); break;
-    case 1: printPgmString(PSTR("idl delay")); break; 
-    case 2: printPgmString(PSTR("stp inv")); break;
-    case 3: printPgmString(PSTR("dir inv")); break;
-    case 4: printPgmString(PSTR("stp en inv")); break;
-    case 5: printPgmString(PSTR("lim inv")); break;
-    case 6: printPgmString(PSTR("prb inv")); break;
-    case 10: printPgmString(PSTR("rpt")); break;
-    case 11: printPgmString(PSTR("jnc dev")); break;
-    case 12: printPgmString(PSTR("arc tol")); break;
-    case 13: printPgmString(PSTR("rpt inch")); break;
-    case 20: printPgmString(PSTR("sft lim")); break;
-    case 21: printPgmString(PSTR("hrd lim")); break;
-    case 22: printPgmString(PSTR("hm cyc")); break;
-    case 23: printPgmString(PSTR("hm dir inv")); break;
-    case 24: printPgmString(PSTR("hm feed")); break;
-    case 25: printPgmString(PSTR("hm seek")); break;
-    case 26: printPgmString(PSTR("hm delay")); break;
-    case 27: printPgmString(PSTR("hm pulloff")); break;
-    case 30: printPgmString(PSTR("rpm max")); break;
-    case 31: printPgmString(PSTR("rpm min")); break;
-    case 32: printPgmString(PSTR("laser")); break;
+    case 0: printPgmString(("stp pulse")); break;
+    case 1: printPgmString(("idl delay")); break; 
+    case 2: printPgmString(("stp inv")); break;
+    case 3: printPgmString(("dir inv")); break;
+    case 4: printPgmString(("stp en inv")); break;
+    case 5: printPgmString(("lim inv")); break;
+    case 6: printPgmString(("prb inv")); break;
+    case 10: printPgmString(("rpt")); break;
+    case 11: printPgmString(("jnc dev")); break;
+    case 12: printPgmString(("arc tol")); break;
+    case 13: printPgmString(("rpt inch")); break;
+    case 20: printPgmString(("sft lim")); break;
+    case 21: printPgmString(("hrd lim")); break;
+    case 22: printPgmString(("hm cyc")); break;
+    case 23: printPgmString(("hm dir inv")); break;
+    case 24: printPgmString(("hm feed")); break;
+    case 25: printPgmString(("hm seek")); break;
+    case 26: printPgmString(("hm delay")); break;
+    case 27: printPgmString(("hm pulloff")); break;
+    case 30: printPgmString(("rpm max")); break;
+    case 31: printPgmString(("rpm min")); break;
+    case 32: printPgmString(("laser")); break;
     default:
       n -= AXIS_SETTINGS_START_VAL;
       uint8_t idx = 0;
@@ -80,10 +80,10 @@ static void report_util_setting_string(uint8_t n) {
       }
       serial_write(n+'x');
       switch (idx) {
-        case 0: printPgmString(PSTR(":stp/mm")); break;
-        case 1: printPgmString(PSTR(":mm/min")); break;
-        case 2: printPgmString(PSTR(":mm/s^2")); break;
-        case 3: printPgmString(PSTR(":mm max")); break;
+        case 0: printPgmString((":stp/mm")); break;
+        case 1: printPgmString((":mm/min")); break;
+        case 2: printPgmString((":mm/s^2")); break;
+        case 3: printPgmString((":mm max")); break;
       }
       break;
   }
@@ -113,9 +113,9 @@ void report_status_message(uint8_t status_code)
 {
   switch(status_code) {
     case STATUS_OK: // STATUS_OK
-      printPgmString(PSTR("ok\r\n")); break;
+      printPgmString(("ok\r\n")); break;
     default:
-      printPgmString(PSTR("error:"));
+      printPgmString(("error:"));
       print_uint8_base10(status_code);
       report_util_line_feed();
   }
@@ -124,7 +124,7 @@ void report_status_message(uint8_t status_code)
 // Prints alarm messages.
 void report_alarm_message(uint8_t alarm_code)
 {
-  printPgmString(PSTR("ALARM:"));
+  printPgmString(("ALARM:"));
   print_uint8_base10(alarm_code);
   report_util_line_feed();
   delay_ms(500); // Force delay to ensure message clears serial write buffer.
@@ -137,30 +137,30 @@ void report_alarm_message(uint8_t alarm_code)
 // is installed, the message number codes are less than zero.
 void report_feedback_message(uint8_t message_code)
 {
-  printPgmString(PSTR("[MSG:"));
+  printPgmString(("[MSG:"));
   switch(message_code) {
     case MESSAGE_CRITICAL_EVENT:
-      printPgmString(PSTR("Reset to continue")); break;
+      printPgmString(("Reset to continue")); break;
     case MESSAGE_ALARM_LOCK:
-      printPgmString(PSTR("'$H'|'$X' to unlock")); break;
+      printPgmString(("'$H'|'$X' to unlock")); break;
     case MESSAGE_ALARM_UNLOCK:
-      printPgmString(PSTR("Caution: Unlocked")); break;
+      printPgmString(("Caution: Unlocked")); break;
     case MESSAGE_ENABLED:
-      printPgmString(PSTR("Enabled")); break;
+      printPgmString(("Enabled")); break;
     case MESSAGE_DISABLED:
-      printPgmString(PSTR("Disabled")); break;
+      printPgmString(("Disabled")); break;
     case MESSAGE_SAFETY_DOOR_AJAR:
-      printPgmString(PSTR("Check Door")); break;
+      printPgmString(("Check Door")); break;
     case MESSAGE_CHECK_LIMITS:
-      printPgmString(PSTR("Check Limits")); break;
+      printPgmString(("Check Limits")); break;
     case MESSAGE_PROGRAM_END:
-      printPgmString(PSTR("Pgm End")); break;
+      printPgmString(("Pgm End")); break;
     case MESSAGE_RESTORE_DEFAULTS:
-      printPgmString(PSTR("Restoring defaults")); break;
+      printPgmString(("Restoring defaults")); break;
     case MESSAGE_SPINDLE_RESTORE:
-      printPgmString(PSTR("Restoring spindle")); break;
+      printPgmString(("Restoring spindle")); break;
     case MESSAGE_SLEEP_MODE:
-      printPgmString(PSTR("Sleeping")); break;
+      printPgmString(("Sleeping")); break;
   }
   report_util_feedback_line_feed();
 }
@@ -169,12 +169,12 @@ void report_feedback_message(uint8_t message_code)
 // Welcome message
 void report_init_message()
 {
-  printPgmString(PSTR("\r\nGrbl " GRBL_VERSION " ['$' for help]\r\n"));
+  printPgmString(("\r\nGrbl " GRBL_VERSION " ['$' for help]\r\n"));
 }
 
 // Grbl help message
 void report_grbl_help() {
-  printPgmString(PSTR("[HLP:$$ $# $G $I $N $x=val $Nx=line $J=line $SLP $C $X $H ~ ! ? ctrl-x]\r\n"));    
+  printPgmString(("[HLP:$$ $# $G $I $N $x=val $Nx=line $J=line $SLP $C $X $H ~ ! ? ctrl-x]\r\n"));    
 }
 
 
@@ -231,7 +231,7 @@ void report_grbl_settings() {
 void report_probe_parameters()
 {
   // Report in terms of machine position.
-  printPgmString(PSTR("[PRB:"));
+  printPgmString(("[PRB:"));
   float print_position[N_AXIS];
   system_convert_array_steps_to_mpos(print_position,sys_probe_position);
   report_util_axis_values(print_position);
@@ -251,20 +251,20 @@ void report_ngc_parameters()
       report_status_message(STATUS_SETTING_READ_FAIL);
       return;
     }
-    printPgmString(PSTR("[G"));
+    printPgmString(("[G"));
     switch (coord_select) {
-      case 6: printPgmString(PSTR("28")); break;
-      case 7: printPgmString(PSTR("30")); break;
+      case 6: printPgmString(("28")); break;
+      case 7: printPgmString(("30")); break;
       default: print_uint8_base10(coord_select+54); break; // G54-G59
     }
     serial_write(':');
     report_util_axis_values(coord_data);
     report_util_feedback_line_feed();
   }
-  printPgmString(PSTR("[G92:")); // Print G92,G92.1 which are not persistent in memory
+  printPgmString(("[G92:")); // Print G92,G92.1 which are not persistent in memory
   report_util_axis_values(gc_state.coord_offset);
   report_util_feedback_line_feed();
-  printPgmString(PSTR("[TLO:")); // Print tool length offset value
+  printPgmString(("[TLO:")); // Print tool length offset value
   printFloat_CoordValue(gc_state.tool_length_offset);
   report_util_feedback_line_feed();
   report_probe_parameters(); // Print probe parameters. Not persistent in memory.
@@ -274,9 +274,9 @@ void report_ngc_parameters()
 // Print current gcode parser mode state
 void report_gcode_modes()
 {
-  printPgmString(PSTR("[GC:G"));
+  printPgmString(("[GC:G"));
   if (gc_state.modal.motion >= MOTION_MODE_PROBE_TOWARD) {
-    printPgmString(PSTR("38."));
+    printPgmString(("38."));
     print_uint8_base10(gc_state.modal.motion - (MOTION_MODE_PROBE_TOWARD-2));
   } else {
     print_uint8_base10(gc_state.modal.motion);
@@ -334,14 +334,14 @@ void report_gcode_modes()
     }
   #endif
   
-  printPgmString(PSTR(" T"));
+  printPgmString((" T"));
   print_uint8_base10(gc_state.tool);
 
-  printPgmString(PSTR(" F"));
+  printPgmString((" F"));
   printFloat_RateValue(gc_state.feed_rate);
 
   #ifdef VARIABLE_SPINDLE
-    printPgmString(PSTR(" S"));
+    printPgmString((" S"));
     printFloat(gc_state.spindle_speed,N_DECIMAL_RPMVALUE);
   #endif
 
@@ -351,7 +351,7 @@ void report_gcode_modes()
 // Prints specified startup line
 void report_startup_line(uint8_t n, char *line)
 {
-  printPgmString(PSTR("$N"));
+  printPgmString(("$N"));
   print_uint8_base10(n);
   serial_write('=');
   printString(line);
@@ -369,10 +369,10 @@ void report_execute_startup_message(char *line, uint8_t status_code)
 // Prints build info line
 void report_build_info(char *line)
 {
-  printPgmString(PSTR("[VER:" GRBL_VERSION "." GRBL_VERSION_BUILD ":"));
+  printPgmString(("[VER:" GRBL_VERSION "." GRBL_VERSION_BUILD ":"));
   printString(line);
   report_util_feedback_line_feed();
-  printPgmString(PSTR("[OPT:")); // Generate compile-time build option list
+  printPgmString(("[OPT:")); // Generate compile-time build option list
   #ifdef VARIABLE_SPINDLE
     serial_write('V');
   #endif
@@ -453,7 +453,7 @@ void report_build_info(char *line)
 // and has been sent into protocol_execute_line() routine to be executed by Grbl.
 void report_echo_line_received(char *line)
 {
-  printPgmString(PSTR("[echo: ")); printString(line);
+  printPgmString(("[echo: ")); printString(line);
   report_util_feedback_line_feed();
 }
 
@@ -474,21 +474,21 @@ void report_realtime_status()
   // Report current machine state and sub-states
   serial_write('<');
   switch (sys.state) {
-    case STATE_IDLE: printPgmString(PSTR("Idle")); break;
-    case STATE_CYCLE: printPgmString(PSTR("Run")); break;
+    case STATE_IDLE: printPgmString(("Idle")); break;
+    case STATE_CYCLE: printPgmString(("Run")); break;
     case STATE_HOLD:
       if (!(sys.suspend & SUSPEND_JOG_CANCEL)) {
-        printPgmString(PSTR("Hold:"));
+        printPgmString(("Hold:"));
         if (sys.suspend & SUSPEND_HOLD_COMPLETE) { serial_write('0'); } // Ready to resume
         else { serial_write('1'); } // Actively holding
         break;
       } // Continues to print jog state during jog cancel.
-    case STATE_JOG: printPgmString(PSTR("Jog")); break;
-    case STATE_HOMING: printPgmString(PSTR("Home")); break;
-    case STATE_ALARM: printPgmString(PSTR("Alarm")); break;
-    case STATE_CHECK_MODE: printPgmString(PSTR("Check")); break;
+    case STATE_JOG: printPgmString(("Jog")); break;
+    case STATE_HOMING: printPgmString(("Home")); break;
+    case STATE_ALARM: printPgmString(("Alarm")); break;
+    case STATE_CHECK_MODE: printPgmString(("Check")); break;
     case STATE_SAFETY_DOOR:
-      printPgmString(PSTR("Door:"));
+      printPgmString(("Door:"));
       if (sys.suspend & SUSPEND_INITIATE_RESTORE) {
         serial_write('3'); // Restoring
       } else {
@@ -503,7 +503,7 @@ void report_realtime_status()
         }
       }
       break;
-    case STATE_SLEEP: printPgmString(PSTR("Sleep")); break;
+    case STATE_SLEEP: printPgmString(("Sleep")); break;
   }
 
   float wco[N_AXIS];
@@ -521,16 +521,16 @@ void report_realtime_status()
 
   // Report machine position
   if (bit_istrue(settings.status_report_mask,BITFLAG_RT_STATUS_POSITION_TYPE)) {
-    printPgmString(PSTR("|MPos:"));
+    printPgmString(("|MPos:"));
   } else {
-    printPgmString(PSTR("|WPos:"));
+    printPgmString(("|WPos:"));
   }
   report_util_axis_values(print_position);
 
   // Returns planner and serial read buffer states.
   #ifdef REPORT_FIELD_BUFFER_STATE
     if (bit_istrue(settings.status_report_mask,BITFLAG_RT_STATUS_BUFFER_STATE)) {
-      printPgmString(PSTR("|Bf:"));
+      printPgmString(("|Bf:"));
       print_uint8_base10(plan_get_block_buffer_available());
       serial_write(',');
       print_uint8_base10(serial_get_rx_buffer_available());
@@ -544,7 +544,7 @@ void report_realtime_status()
       if (cur_block != NULL) {
         uint32_t ln = cur_block->line_number;
         if (ln > 0) {
-          printPgmString(PSTR("|Ln:"));
+          printPgmString(("|Ln:"));
           printInteger(ln);
         }
       }
@@ -554,12 +554,12 @@ void report_realtime_status()
   // Report realtime feed speed
   #ifdef REPORT_FIELD_CURRENT_FEED_SPEED
     #ifdef VARIABLE_SPINDLE
-      printPgmString(PSTR("|FS:"));
+      printPgmString(("|FS:"));
       printFloat_RateValue(st_get_realtime_rate());
       serial_write(',');
       printFloat(sys.spindle_speed,N_DECIMAL_RPMVALUE);
     #else
-      printPgmString(PSTR("|F:"));
+      printPgmString(("|F:"));
       printFloat_RateValue(st_get_realtime_rate());
     #endif      
   #endif
@@ -569,7 +569,7 @@ void report_realtime_status()
     uint8_t ctrl_pin_state = system_control_get_state();
     uint8_t prb_pin_state = probe_get_state();
     if (lim_pin_state | ctrl_pin_state | prb_pin_state) {
-      printPgmString(PSTR("|Pn:"));
+      printPgmString(("|Pn:"));
       if (prb_pin_state) { serial_write('P'); }
       if (lim_pin_state) {
         #ifdef ENABLE_DUAL_AXIS
@@ -606,7 +606,7 @@ void report_realtime_status()
         sys.report_wco_counter = (REPORT_WCO_REFRESH_BUSY_COUNT-1); // Reset counter for slow refresh
       } else { sys.report_wco_counter = (REPORT_WCO_REFRESH_IDLE_COUNT-1); }
       if (sys.report_ovr_counter == 0) { sys.report_ovr_counter = 1; } // Set override on next report.
-      printPgmString(PSTR("|WCO:"));
+      printPgmString(("|WCO:"));
       report_util_axis_values(wco);
     }
   #endif
@@ -617,7 +617,7 @@ void report_realtime_status()
       if (sys.state & (STATE_HOMING | STATE_CYCLE | STATE_HOLD | STATE_JOG | STATE_SAFETY_DOOR)) {
         sys.report_ovr_counter = (REPORT_OVR_REFRESH_BUSY_COUNT-1); // Reset counter for slow refresh
       } else { sys.report_ovr_counter = (REPORT_OVR_REFRESH_IDLE_COUNT-1); }
-      printPgmString(PSTR("|Ov:"));
+      printPgmString(("|Ov:"));
       print_uint8_base10(sys.f_override);
       serial_write(',');
       print_uint8_base10(sys.r_override);
@@ -627,7 +627,7 @@ void report_realtime_status()
       uint8_t sp_state = spindle_get_state();
       uint8_t cl_state = coolant_get_state();
       if (sp_state || cl_state) {
-        printPgmString(PSTR("|A:"));
+        printPgmString(("|A:"));
         if (sp_state) { // != SPINDLE_STATE_DISABLE
           #ifdef VARIABLE_SPINDLE 
             #ifdef USE_SPINDLE_DIR_AS_ENABLE_PIN
diff --git a/grbl/serial.c b/grbl/serial.c
index cf5f35e..2c2c496 100644
--- a/grbl/serial.c
+++ b/grbl/serial.c
@@ -36,6 +36,7 @@ volatile uint8_t serial_tx_buffer_tail = 0;
 // Returns the number of bytes available in the RX serial buffer.
 uint8_t serial_get_rx_buffer_available()
 {
+  printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);  
   uint8_t rtail = serial_rx_buffer_tail; // Copy to limit multiple calls to volatile
   if (serial_rx_buffer_head >= rtail) { return(RX_BUFFER_SIZE - (serial_rx_buffer_head-rtail)); }
   return((rtail-serial_rx_buffer_head-1));
@@ -46,6 +47,7 @@ uint8_t serial_get_rx_buffer_available()
 // NOTE: Deprecated. Not used unless classic status reports are enabled in config.h.
 uint8_t serial_get_rx_buffer_count()
 {
+  printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);  
   uint8_t rtail = serial_rx_buffer_tail; // Copy to limit multiple calls to volatile
   if (serial_rx_buffer_head >= rtail) { return(serial_rx_buffer_head-rtail); }
   return (RX_BUFFER_SIZE - (rtail-serial_rx_buffer_head));
@@ -56,6 +58,7 @@ uint8_t serial_get_rx_buffer_count()
 // NOTE: Not used except for debugging and ensuring no TX bottlenecks.
 uint8_t serial_get_tx_buffer_count()
 {
+  printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);  
   uint8_t ttail = serial_tx_buffer_tail; // Copy to limit multiple calls to volatile
   if (serial_tx_buffer_head >= ttail) { return(serial_tx_buffer_head-ttail); }
   return (TX_RING_BUFFER - (ttail-serial_tx_buffer_head));
@@ -64,19 +67,21 @@ uint8_t serial_get_tx_buffer_count()
 
 void serial_init()
 {
-  // Set baud rate
-  #if BAUD_RATE < 57600
-    uint16_t UBRR0_value = ((F_CPU / (8L * BAUD_RATE)) - 1)/2 ;
-    UCSR0A &= ~(1 << U2X0); // baud doubler off  - Only needed on Uno XXX
-  #else
-    uint16_t UBRR0_value = ((F_CPU / (4L * BAUD_RATE)) - 1)/2;
-    UCSR0A |= (1 << U2X0);  // baud doubler on for high baud rates, i.e. 115200
-  #endif
-  UBRR0H = UBRR0_value >> 8;
-  UBRR0L = UBRR0_value;
+  printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);  
 
-  // enable rx, tx, and interrupt on complete reception of a byte
-  UCSR0B |= (1<<RXEN0 | 1<<TXEN0 | 1<<RXCIE0);
+  // Set baud rate
+  //#if BAUD_RATE < 57600
+  //  uint16_t UBRR0_value = ((F_CPU / (8L * BAUD_RATE)) - 1)/2 ;
+  //  UCSR0A &= ~(1 << U2X0); // baud doubler off  - Only needed on Uno XXX
+  //#else
+  //  uint16_t UBRR0_value = ((F_CPU / (4L * BAUD_RATE)) - 1)/2;
+  //  UCSR0A |= (1 << U2X0);  // baud doubler on for high baud rates, i.e. 115200
+  //#endif
+  //UBRR0H = UBRR0_value >> 8;
+  //UBRR0L = UBRR0_value;
+//
+  //// enable rx, tx, and interrupt on complete reception of a byte
+  //UCSR0B |= (1<<RXEN0 | 1<<TXEN0 | 1<<RXCIE0);
 
   // defaults to 8-bit, no parity, 1 stop bit
 }
@@ -84,6 +89,8 @@ void serial_init()
 
 // Writes one byte to the TX serial buffer. Called by main program.
 void serial_write(uint8_t data) {
+  printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);  
+
   // Calculate next head
   uint8_t next_head = serial_tx_buffer_head + 1;
   if (next_head == TX_RING_BUFFER) { next_head = 0; }
@@ -99,32 +106,33 @@ void serial_write(uint8_t data) {
   serial_tx_buffer_head = next_head;
 
   // Enable Data Register Empty Interrupt to make sure tx-streaming is running
-  UCSR0B |=  (1 << UDRIE0);
+  //UCSR0B |=  (1 << UDRIE0);
 }
 
 
 // Data Register Empty Interrupt handler
-ISR(SERIAL_UDRE)
-{
-  uint8_t tail = serial_tx_buffer_tail; // Temporary serial_tx_buffer_tail (to optimize for volatile)
-
-  // Send a byte from the buffer
-  UDR0 = serial_tx_buffer[tail];
-
-  // Update tail position
-  tail++;
-  if (tail == TX_RING_BUFFER) { tail = 0; }
-
-  serial_tx_buffer_tail = tail;
-
-  // Turn off Data Register Empty Interrupt to stop tx-streaming if this concludes the transfer
-  if (tail == serial_tx_buffer_head) { UCSR0B &= ~(1 << UDRIE0); }
-}
+//ISR(SERIAL_UDRE)
+//{
+//  uint8_t tail = serial_tx_buffer_tail; // Temporary serial_tx_buffer_tail (to optimize for volatile)
+//
+//  // Send a byte from the buffer
+//  UDR0 = serial_tx_buffer[tail];
+//
+//  // Update tail position
+//  tail++;
+//  if (tail == TX_RING_BUFFER) { tail = 0; }
+//
+//  serial_tx_buffer_tail = tail;
+//
+//  // Turn off Data Register Empty Interrupt to stop tx-streaming if this concludes the transfer
+//  if (tail == serial_tx_buffer_head) { UCSR0B &= ~(1 << UDRIE0); }
+//}
 
 
 // Fetches the first byte in the serial read buffer. Called by main program.
 uint8_t serial_read()
 {
+  printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);  
   uint8_t tail = serial_rx_buffer_tail; // Temporary serial_rx_buffer_tail (to optimize for volatile)
   if (serial_rx_buffer_head == tail) {
     return SERIAL_NO_DATA;
@@ -140,63 +148,63 @@ uint8_t serial_read()
 }
 
 
-ISR(SERIAL_RX)
-{
-  uint8_t data = UDR0;
-  uint8_t next_head;
-
-  // Pick off realtime command characters directly from the serial stream. These characters are
-  // not passed into the main buffer, but these set system state flag bits for realtime execution.
-  switch (data) {
-    case CMD_RESET:         mc_reset(); break; // Call motion control reset routine.
-    case CMD_STATUS_REPORT: system_set_exec_state_flag(EXEC_STATUS_REPORT); break; // Set as true
-    case CMD_CYCLE_START:   system_set_exec_state_flag(EXEC_CYCLE_START); break; // Set as true
-    case CMD_FEED_HOLD:     system_set_exec_state_flag(EXEC_FEED_HOLD); break; // Set as true
-    default :
-      if (data > 0x7F) { // Real-time control characters are extended ACSII only.
-        switch(data) {
-          case CMD_SAFETY_DOOR:   system_set_exec_state_flag(EXEC_SAFETY_DOOR); break; // Set as true
-          case CMD_JOG_CANCEL:   
-            if (sys.state & STATE_JOG) { // Block all other states from invoking motion cancel.
-              system_set_exec_state_flag(EXEC_MOTION_CANCEL); 
-            }
-            break; 
-          #ifdef DEBUG
-            case CMD_DEBUG_REPORT: {uint8_t sreg = SREG; cli(); bit_true(sys_rt_exec_debug,EXEC_DEBUG_REPORT); SREG = sreg;} break;
-          #endif
-          case CMD_FEED_OVR_RESET: system_set_exec_motion_override_flag(EXEC_FEED_OVR_RESET); break;
-          case CMD_FEED_OVR_COARSE_PLUS: system_set_exec_motion_override_flag(EXEC_FEED_OVR_COARSE_PLUS); break;
-          case CMD_FEED_OVR_COARSE_MINUS: system_set_exec_motion_override_flag(EXEC_FEED_OVR_COARSE_MINUS); break;
-          case CMD_FEED_OVR_FINE_PLUS: system_set_exec_motion_override_flag(EXEC_FEED_OVR_FINE_PLUS); break;
-          case CMD_FEED_OVR_FINE_MINUS: system_set_exec_motion_override_flag(EXEC_FEED_OVR_FINE_MINUS); break;
-          case CMD_RAPID_OVR_RESET: system_set_exec_motion_override_flag(EXEC_RAPID_OVR_RESET); break;
-          case CMD_RAPID_OVR_MEDIUM: system_set_exec_motion_override_flag(EXEC_RAPID_OVR_MEDIUM); break;
-          case CMD_RAPID_OVR_LOW: system_set_exec_motion_override_flag(EXEC_RAPID_OVR_LOW); break;
-          case CMD_SPINDLE_OVR_RESET: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_RESET); break;
-          case CMD_SPINDLE_OVR_COARSE_PLUS: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_COARSE_PLUS); break;
-          case CMD_SPINDLE_OVR_COARSE_MINUS: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_COARSE_MINUS); break;
-          case CMD_SPINDLE_OVR_FINE_PLUS: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_FINE_PLUS); break;
-          case CMD_SPINDLE_OVR_FINE_MINUS: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_FINE_MINUS); break;
-          case CMD_SPINDLE_OVR_STOP: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_STOP); break;
-          case CMD_COOLANT_FLOOD_OVR_TOGGLE: system_set_exec_accessory_override_flag(EXEC_COOLANT_FLOOD_OVR_TOGGLE); break;
-          #ifdef ENABLE_M7
-            case CMD_COOLANT_MIST_OVR_TOGGLE: system_set_exec_accessory_override_flag(EXEC_COOLANT_MIST_OVR_TOGGLE); break;
-          #endif
-        }
-        // Throw away any unfound extended-ASCII character by not passing it to the serial buffer.
-      } else { // Write character to buffer
-        next_head = serial_rx_buffer_head + 1;
-        if (next_head == RX_RING_BUFFER) { next_head = 0; }
-
-        // Write data to buffer unless it is full.
-        if (next_head != serial_rx_buffer_tail) {
-          serial_rx_buffer[serial_rx_buffer_head] = data;
-          serial_rx_buffer_head = next_head;
-        }
-      }
-  }
-}
-
+//ISR(SERIAL_RX)
+//{
+//  uint8_t data = UDR0;
+//  uint8_t next_head;
+//
+//  // Pick off realtime command characters directly from the serial stream. These characters are
+//  // not passed into the main buffer, but these set system state flag bits for realtime execution.
+//  switch (data) {
+//    case CMD_RESET:         mc_reset(); break; // Call motion control reset routine.
+//    case CMD_STATUS_REPORT: system_set_exec_state_flag(EXEC_STATUS_REPORT); break; // Set as true
+//    case CMD_CYCLE_START:   system_set_exec_state_flag(EXEC_CYCLE_START); break; // Set as true
+//    case CMD_FEED_HOLD:     system_set_exec_state_flag(EXEC_FEED_HOLD); break; // Set as true
+//    default :
+//      if (data > 0x7F) { // Real-time control characters are extended ACSII only.
+//        switch(data) {
+//          case CMD_SAFETY_DOOR:   system_set_exec_state_flag(EXEC_SAFETY_DOOR); break; // Set as true
+//          case CMD_JOG_CANCEL:   
+//            if (sys.state & STATE_JOG) { // Block all other states from invoking motion cancel.
+//              system_set_exec_state_flag(EXEC_MOTION_CANCEL); 
+//            }
+//            break; 
+//          #ifdef DEBUG
+//            case CMD_DEBUG_REPORT: {uint8_t sreg = SREG; cli(); bit_true(sys_rt_exec_debug,EXEC_DEBUG_REPORT); SREG = sreg;} break;
+//          #endif
+//          case CMD_FEED_OVR_RESET: system_set_exec_motion_override_flag(EXEC_FEED_OVR_RESET); break;
+//          case CMD_FEED_OVR_COARSE_PLUS: system_set_exec_motion_override_flag(EXEC_FEED_OVR_COARSE_PLUS); break;
+//          case CMD_FEED_OVR_COARSE_MINUS: system_set_exec_motion_override_flag(EXEC_FEED_OVR_COARSE_MINUS); break;
+//          case CMD_FEED_OVR_FINE_PLUS: system_set_exec_motion_override_flag(EXEC_FEED_OVR_FINE_PLUS); break;
+//          case CMD_FEED_OVR_FINE_MINUS: system_set_exec_motion_override_flag(EXEC_FEED_OVR_FINE_MINUS); break;
+//          case CMD_RAPID_OVR_RESET: system_set_exec_motion_override_flag(EXEC_RAPID_OVR_RESET); break;
+//          case CMD_RAPID_OVR_MEDIUM: system_set_exec_motion_override_flag(EXEC_RAPID_OVR_MEDIUM); break;
+//          case CMD_RAPID_OVR_LOW: system_set_exec_motion_override_flag(EXEC_RAPID_OVR_LOW); break;
+//          case CMD_SPINDLE_OVR_RESET: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_RESET); break;
+//          case CMD_SPINDLE_OVR_COARSE_PLUS: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_COARSE_PLUS); break;
+//          case CMD_SPINDLE_OVR_COARSE_MINUS: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_COARSE_MINUS); break;
+//          case CMD_SPINDLE_OVR_FINE_PLUS: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_FINE_PLUS); break;
+//          case CMD_SPINDLE_OVR_FINE_MINUS: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_FINE_MINUS); break;
+//          case CMD_SPINDLE_OVR_STOP: system_set_exec_accessory_override_flag(EXEC_SPINDLE_OVR_STOP); break;
+//          case CMD_COOLANT_FLOOD_OVR_TOGGLE: system_set_exec_accessory_override_flag(EXEC_COOLANT_FLOOD_OVR_TOGGLE); break;
+//          #ifdef ENABLE_M7
+//            case CMD_COOLANT_MIST_OVR_TOGGLE: system_set_exec_accessory_override_flag(EXEC_COOLANT_MIST_OVR_TOGGLE); break;
+//          #endif
+//        }
+//        // Throw away any unfound extended-ASCII character by not passing it to the serial buffer.
+//      } else { // Write character to buffer
+//        next_head = serial_rx_buffer_head + 1;
+//        if (next_head == RX_RING_BUFFER) { next_head = 0; }
+//
+//        // Write data to buffer unless it is full.
+//        if (next_head != serial_rx_buffer_tail) {
+//          serial_rx_buffer[serial_rx_buffer_head] = data;
+//          serial_rx_buffer_head = next_head;
+//        }
+//      }
+//  }
+//}
+//
 
 void serial_reset_read_buffer()
 {
diff --git a/grbl/system.c b/grbl/system.c
index 570d6da..8482f04 100644
--- a/grbl/system.c
+++ b/grbl/system.c
@@ -23,14 +23,16 @@
 
 void system_init()
 {
-  CONTROL_DDR &= ~(CONTROL_MASK); // Configure as input pins
-  #ifdef DISABLE_CONTROL_PIN_PULL_UP
-    CONTROL_PORT &= ~(CONTROL_MASK); // Normal low operation. Requires external pull-down.
-  #else
-    CONTROL_PORT |= CONTROL_MASK;   // Enable internal pull-up resistors. Normal high operation.
-  #endif
-  CONTROL_PCMSK |= CONTROL_MASK;  // Enable specific pins of the Pin Change Interrupt
-  PCICR |= (1 << CONTROL_INT);   // Enable Pin Change Interrupt
+  printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
+
+  //CONTROL_DDR &= ~(CONTROL_MASK); // Configure as input pins
+  //#ifdef DISABLE_CONTROL_PIN_PULL_UP
+  //  CONTROL_PORT &= ~(CONTROL_MASK); // Normal low operation. Requires external pull-down.
+  //#else
+  //  CONTROL_PORT |= CONTROL_MASK;   // Enable internal pull-up resistors. Normal high operation.
+  //#endif
+  //CONTROL_PCMSK |= CONTROL_MASK;  // Enable specific pins of the Pin Change Interrupt
+  //PCICR |= (1 << CONTROL_INT);   // Enable Pin Change Interrupt
 }
 
 
@@ -39,21 +41,23 @@ void system_init()
 // defined by the CONTROL_PIN_INDEX in the header file.
 uint8_t system_control_get_state()
 {
-  uint8_t control_state = 0;
-  uint8_t pin = (CONTROL_PIN & CONTROL_MASK) ^ CONTROL_MASK;
-  #ifdef INVERT_CONTROL_PIN_MASK
-    pin ^= INVERT_CONTROL_PIN_MASK;
-  #endif
-  if (pin) {
-    #ifdef ENABLE_SAFETY_DOOR_INPUT_PIN
-      if (bit_istrue(pin,(1<<CONTROL_SAFETY_DOOR_BIT))) { control_state |= CONTROL_PIN_INDEX_SAFETY_DOOR; }
-    #else
-      if (bit_istrue(pin,(1<<CONTROL_FEED_HOLD_BIT))) { control_state |= CONTROL_PIN_INDEX_FEED_HOLD; }
-    #endif
-    if (bit_istrue(pin,(1<<CONTROL_RESET_BIT))) { control_state |= CONTROL_PIN_INDEX_RESET; }
-    if (bit_istrue(pin,(1<<CONTROL_CYCLE_START_BIT))) { control_state |= CONTROL_PIN_INDEX_CYCLE_START; }
-  }
-  return(control_state);
+  printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
+  return 0;
+//  uint8_t control_state = 0;
+//  uint8_t pin = (CONTROL_PIN & CONTROL_MASK) ^ CONTROL_MASK;
+//  #ifdef INVERT_CONTROL_PIN_MASK
+//    pin ^= INVERT_CONTROL_PIN_MASK;
+//  #endif
+//  if (pin) {
+//    #ifdef ENABLE_SAFETY_DOOR_INPUT_PIN
+//      if (bit_istrue(pin,(1<<CONTROL_SAFETY_DOOR_BIT))) { control_state |= CONTROL_PIN_INDEX_SAFETY_DOOR; }
+//    #else
+//      if (bit_istrue(pin,(1<<CONTROL_FEED_HOLD_BIT))) { control_state |= CONTROL_PIN_INDEX_FEED_HOLD; }
+//    #endif
+//    if (bit_istrue(pin,(1<<CONTROL_RESET_BIT))) { control_state |= CONTROL_PIN_INDEX_RESET; }
+//    if (bit_istrue(pin,(1<<CONTROL_CYCLE_START_BIT))) { control_state |= CONTROL_PIN_INDEX_CYCLE_START; }
+//  }
+//  return(control_state);
 }
 
 
@@ -61,42 +65,46 @@ uint8_t system_control_get_state()
 // only the realtime command execute variable to have the main program execute these when
 // its ready. This works exactly like the character-based realtime commands when picked off
 // directly from the incoming serial data stream.
-ISR(CONTROL_INT_vect)
-{
-  uint8_t pin = system_control_get_state();
-  if (pin) {
-    if (bit_istrue(pin,CONTROL_PIN_INDEX_RESET)) {
-      mc_reset();
-    }
-    if (bit_istrue(pin,CONTROL_PIN_INDEX_CYCLE_START)) {
-      bit_true(sys_rt_exec_state, EXEC_CYCLE_START);
-    }
-    #ifndef ENABLE_SAFETY_DOOR_INPUT_PIN
-      if (bit_istrue(pin,CONTROL_PIN_INDEX_FEED_HOLD)) {
-        bit_true(sys_rt_exec_state, EXEC_FEED_HOLD);
-    #else
-      if (bit_istrue(pin,CONTROL_PIN_INDEX_SAFETY_DOOR)) {
-        bit_true(sys_rt_exec_state, EXEC_SAFETY_DOOR);
-    #endif
-    }
-  }
-}
+//ISR(CONTROL_INT_vect)
+//{
+//  uint8_t pin = system_control_get_state();
+//  if (pin) {
+//    if (bit_istrue(pin,CONTROL_PIN_INDEX_RESET)) {
+//      mc_reset();
+//    }
+//    if (bit_istrue(pin,CONTROL_PIN_INDEX_CYCLE_START)) {
+//      bit_true(sys_rt_exec_state, EXEC_CYCLE_START);
+//    }
+//    #ifndef ENABLE_SAFETY_DOOR_INPUT_PIN
+//      if (bit_istrue(pin,CONTROL_PIN_INDEX_FEED_HOLD)) {
+//        bit_true(sys_rt_exec_state, EXEC_FEED_HOLD);
+//    #else
+//      if (bit_istrue(pin,CONTROL_PIN_INDEX_SAFETY_DOOR)) {
+//        bit_true(sys_rt_exec_state, EXEC_SAFETY_DOOR);
+//    #endif
+//    }
+//  }
+//}
 
 
 // Returns if safety door is ajar(T) or closed(F), based on pin state.
 uint8_t system_check_safety_door_ajar()
 {
-  #ifdef ENABLE_SAFETY_DOOR_INPUT_PIN
-    return(system_control_get_state() & CONTROL_PIN_INDEX_SAFETY_DOOR);
-  #else
-    return(false); // Input pin not enabled, so just return that it's closed.
-  #endif
+  printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
+  return 0;
+
+  //#ifdef ENABLE_SAFETY_DOOR_INPUT_PIN
+  //  return(system_control_get_state() & CONTROL_PIN_INDEX_SAFETY_DOOR);
+  //#else
+  //  return(false); // Input pin not enabled, so just return that it's closed.
+  //#endif
 }
 
 
 // Executes user startup script, if stored.
 void system_execute_startup(char *line)
 {
+  printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
   uint8_t n;
   for (n=0; n < N_STARTUP_LINE; n++) {
     if (!(settings_read_startup_line(n, line))) {
@@ -121,9 +129,11 @@ void system_execute_startup(char *line)
 // since there are motions already stored in the buffer. However, this 'lag' should not
 // be an issue, since these commands are not typically used during a cycle.
 
-
+//Nice!!
 uint8_t system_execute_line(char *line)
 {
+  printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
+
   uint8_t char_counter = 1;
   uint8_t helper_var = 0; // Helper variable
   float parameter, value;
@@ -355,58 +365,69 @@ uint8_t system_check_travel_limits(float *target)
 
 
 // Special handlers for setting and clearing Grbl's real-time execution flags.
+// ecmc: set execute here (start rt thread maybe)
 void system_set_exec_state_flag(uint8_t mask) {
-  uint8_t sreg = SREG;
-  cli();
+  printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
+
+  //uint8_t sreg = SREG;
+  //cli();
   sys_rt_exec_state |= (mask);
-  SREG = sreg;
+  //SREG = sreg;
 }
 
 void system_clear_exec_state_flag(uint8_t mask) {
-  uint8_t sreg = SREG;
-  cli();
+  printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
+
+  //uint8_t sreg = SREG;
+  //cli();
   sys_rt_exec_state &= ~(mask);
-  SREG = sreg;
+  //SREG = sreg;
 }
 
 void system_set_exec_alarm(uint8_t code) {
-  uint8_t sreg = SREG;
-  cli();
+  printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
+  //uint8_t sreg = SREG;
+  //cli();
   sys_rt_exec_alarm = code;
-  SREG = sreg;
+  //SREG = sreg;
 }
 
 void system_clear_exec_alarm() {
-  uint8_t sreg = SREG;
-  cli();
+  printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
+  //uint8_t sreg = SREG;
+  //cli();
   sys_rt_exec_alarm = 0;
-  SREG = sreg;
+  //SREG = sreg;
 }
 
 void system_set_exec_motion_override_flag(uint8_t mask) {
-  uint8_t sreg = SREG;
-  cli();
+  printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
+  //uint8_t sreg = SREG;
+  //cli();
   sys_rt_exec_motion_override |= (mask);
-  SREG = sreg;
+  //SREG = sreg;
 }
 
 void system_set_exec_accessory_override_flag(uint8_t mask) {
-  uint8_t sreg = SREG;
-  cli();
+  printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
+  //uint8_t sreg = SREG;
+  //cli();
   sys_rt_exec_accessory_override |= (mask);
-  SREG = sreg;
+  //SREG = sreg;
 }
 
 void system_clear_exec_motion_overrides() {
-  uint8_t sreg = SREG;
-  cli();
+  printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
+  //uint8_t sreg = SREG;
+  //cli();
   sys_rt_exec_motion_override = 0;
-  SREG = sreg;
+  //SREG = sreg;
 }
 
 void system_clear_exec_accessory_overrides() {
-  uint8_t sreg = SREG;
-  cli();
+  printf("%s:%s:%d:\n",__FILE__,__FUNCTION__,__LINE__);
+  //uint8_t sreg = SREG;
+  //cli();
   sys_rt_exec_accessory_override = 0;
-  SREG = sreg;
+  //SREG = sreg;
 }
diff --git a/iocsh/ecmc_grbl_eeprom.txt b/iocsh/ecmc_grbl_eeprom.txt
new file mode 100644
index 0000000..e69de29
diff --git a/iocsh/test.script b/iocsh/test.script
new file mode 100644
index 0000000..8ccd6ad
--- /dev/null
+++ b/iocsh/test.script
@@ -0,0 +1,37 @@
+##############################################################################
+## Example: Demo of ecmc SocketCAN plugin
+## https://github.com/anderssandstrom/e3-ecmc_plugin_socketcan
+##
+## The plugin exposes:
+##
+
+## Initiation:
+epicsEnvSet("IOC" ,"$(IOC="IOC_TEST")")
+epicsEnvSet("ECMCCFG_INIT" ,"")  #Only run startup once (auto at PSI, need call at ESS), variable set to "#" in startup.cmd
+epicsEnvSet("SCRIPTEXEC" ,"$(SCRIPTEXEC="iocshLoad")")
+
+require ecmccfg 7.0.1
+
+# run module startup.cmd (only needed at ESS  PSI auto call at require)
+$(ECMCCFG_INIT)$(SCRIPTEXEC) ${ecmccfg_DIR}startup.cmd, "IOC=$(IOC),ECMC_VER=7.0.1,MASTER_ID=-1"
+
+##############################################################################
+## Configure hardware:
+# No EtherCAT hardware (in this example)..
+
+##############################################################################
+## Load plugin:
+epicsEnvSet("PLUGIN_VER" ,"develop")
+require ecmc_plugin_grbl $(PLUGIN_VER)
+
+epicsEnvSet(ECMC_PLUGIN_FILNAME,"/home/pi/epics/base-7.0.5/require/${E3_REQUIRE_VERSION}/siteMods/ecmc_plugin_grbl/$(PLUGIN_VER)/lib/${EPICS_HOST_ARCH=linux-x86_64}/libecmc_plugin_grbl.so")
+epicsEnvSet(ECMC_PLUGIN_CONFIG,"DBG_PRINT=1;") # Only one option implemented in this plugin
+${SCRIPTEXEC} ${ecmccfg_DIR}loadPlugin.cmd, "PLUGIN_ID=0,FILE=${ECMC_PLUGIN_FILNAME},CONFIG='${ECMC_PLUGIN_CONFIG}', REPORT=1"
+epicsEnvUnset(ECMC_PLUGIN_FILNAME)
+epicsEnvUnset(ECMC_PLUGIN_CONFIG)
+
+##############################################################################
+## PLC 0
+# $(SCRIPTEXEC) $(ecmccfg_DIR)loadPLCFile.cmd, "PLC_ID=0, SAMPLE_RATE_MS=1000,FILE=./plc/can.plc")
+
+iocInit()