diff --git a/commandline/canbus.c b/commandline/canbus.c
index 3ec02fb..694ee58 100644
--- a/commandline/canbus.c
+++ b/commandline/canbus.c
@@ -66,8 +66,7 @@ static int read_ttyX(TTY_descr *d){
         l = 0;
         FD_ZERO(&rfds);
         FD_SET(d->comfd, &rfds);
-        // wait for 10ms
-        tv.tv_sec = 0; tv.tv_usec = 50000;
+        tv.tv_sec = 0; tv.tv_usec = 500;
         retval = select(d->comfd + 1, &rfds, NULL, NULL, &tv);
         if (!retval) break;
         if(FD_ISSET(d->comfd, &rfds)){
@@ -91,16 +90,18 @@ static int ttyWR(const char *buff, int len){
 #ifdef EBUG
     int _U_ n = write(STDERR_FILENO, buff, len);
     fprintf(stderr, "\n");
+    double t0 = dtime();
 #endif
     int w = write_tty(dev->comfd, buff, (size_t)len);
     if(!w) w = write_tty(dev->comfd, "\n", 1);
-    char *s = read_string(); // clear echo
+    DBG("Written, dt=%g", dtime() - t0);
+    char *s = read_string(); // clear echo & check
     if(!s || strcmp(s, buff) != 0){
         WARNX("wrong answer! Got '%s' instead of '%s'", s, buff);
         return 1;
     }
     pthread_mutex_unlock(&mutex);
-    DBG("Success");
+    DBG("Success, dt=%g", dtime() - t0);
     return w;
 }
 
@@ -113,8 +114,7 @@ void setserialspeed(int speed){
 }
 
 static void clearRXbuf(){
-    double t0 = dtime();
-    while(read_string() && dtime() - t0 < T_POLLING_TMOUT){usleep(1000);}
+    while(read_ttyX(dev));
 }
 
 int canbus_open(const char *devname){
@@ -194,7 +194,10 @@ static char *read_string(){
             }
             memcpy(ptr, dev->buf, dev->buflen);
             r += l; LL -= l; ptr += l;
-            if(ptr[-1] == '\n') break;
+            if(ptr[-1] == '\n'){
+                //DBG("Newline detected");
+                break;
+            }
             d0 = dtime();
         }
     }while(dtime() - d0 < WAIT_TMOUT && LL);
@@ -210,7 +213,7 @@ static char *read_string(){
             optr = NULL;
             return NULL;
         }
-        DBG("buf: %s", buf);
+        DBG("buf: %s, time: %g", buf, dtime() - d0);
         return buf;
     }
     return NULL;
@@ -225,7 +228,7 @@ CANmesg *parseCANmesg(const char *str){
     return &m;
 }
 
-#if 0
+#ifdef EBUG
 void showM(CANmesg *m){
     printf("TS=%d, ID=0x%X", m->timemark, m->ID);
     int l = m->len;
@@ -246,8 +249,10 @@ int canbus_read(CANmesg *mesg){
     while(dtime() - t0 < T_POLLING_TMOUT){ // read answer
         if((ans = read_string())){ // parse new data
             if((m = parseCANmesg(ans))){
-                //DBG("Got canbus message:");
-                //showM(m);
+                DBG("Got canbus message (dT=%g):", dtime() - t0);
+#ifdef EBUG
+                showM(m);
+#endif
                 if(ID && m->ID == ID){
                     memcpy(mesg, m, sizeof(CANmesg));
                     DBG("All OK");
diff --git a/commandline/canopen.c b/commandline/canopen.c
index 32ed2e3..52a14dc 100644
--- a/commandline/canopen.c
+++ b/commandline/canopen.c
@@ -259,8 +259,8 @@ int64_t SDO_read(const SDO_dic_entry *e, uint8_t NID){
     return ans;
 }
 
-// write SDO data
-int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, uint8_t *data){
+// write SDO data, return 0 if all OK
+int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data){
     if(!e || !data || e->datasize < 1 || e->datasize > 4){
         WARNX("SDO_write(): bad datalen");
         return 1;
@@ -300,7 +300,7 @@ int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, uint8_t *data){
     return 0;
 }
 
-int SDO_write(const SDO_dic_entry *e, _U_ uint8_t NID, int64_t data){
+int SDO_write(const SDO_dic_entry *e, uint8_t NID, int64_t data){
     if(!e) return 1;
     uint8_t arr[4] = {0};
     uint32_t U;
diff --git a/commandline/canopen.h b/commandline/canopen.h
index 904d717..2c01c78 100644
--- a/commandline/canopen.h
+++ b/commandline/canopen.h
@@ -83,7 +83,7 @@ SDO *readSDOvalue(uint16_t idx, uint8_t subidx, uint8_t NID);
 
 int64_t SDO_read(const SDO_dic_entry *e, uint8_t NID);
 
-int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, uint8_t *data);
+int SDO_writeArr(const SDO_dic_entry *e, uint8_t NID, const uint8_t *data);
 int SDO_write(const SDO_dic_entry *e, uint8_t NID, int64_t data);
 
 //int SDO_readByte(uint16_t idx, uint8_t subidx, uint8_t *data, uint8_t NID);
diff --git a/commandline/cmdlnopts.c b/commandline/cmdlnopts.c
index 77f1cbd..2648b45 100644
--- a/commandline/cmdlnopts.c
+++ b/commandline/cmdlnopts.c
@@ -76,6 +76,7 @@ static myoption cmdlnopts[] = {
     {"check",   NEED_ARG,   NULL,   'k',    arg_string, APTR(&G.checkfile), _("check SDO data file")},
     {"disable", NO_ARGS,    NULL,   'D',    arg_int,    APTR(&G.disable),   _("disable motor")},
     {"readvals",NO_ARGS,    NULL,   'R',    arg_int,    APTR(&G.showpars),  _("read values of used parameters")},
+    {"enablesw",NO_ARGS,    NULL,   'E',    arg_int,    APTR(&G.enableESW), _("enable end-switches 1 and 2")},
    end_option
 };
 
diff --git a/commandline/cmdlnopts.h b/commandline/cmdlnopts.h
index 3688374..9bbee87 100644
--- a/commandline/cmdlnopts.h
+++ b/commandline/cmdlnopts.h
@@ -49,6 +49,7 @@ typedef struct{
     int zeropos;            // set position to zero
     int disable;            // disable motor
     int showpars;           // show values of some parameters
+    int enableESW;          // send signal to enable end-switches
 } glob_pars;
 
 
diff --git a/commandline/main.c b/commandline/main.c
index 8d559d1..0f2a386 100644
--- a/commandline/main.c
+++ b/commandline/main.c
@@ -37,7 +37,6 @@ static uint16_t microstepping = 0;
 void signals(int sig){
     putlog("Exit with status %d", sig);
     DBG("Exit with status %d", sig);
-    restore_console();
     if(GP->pidfile) // remove unnesessary PID file
         unlink(GP->pidfile);
     canbus_close();
@@ -183,46 +182,71 @@ int main(int argc, char *argv[]){
     ID = GP->NodeID;
 #define getSDOe(SDO, fn, e)  do{if(INT64_MIN != (i64 = SDO_read(&SDO, ID))) fn(i64); else ERRX(e);}while(0)
 #define getSDOw(SDO, fn, e)  do{if(INT64_MIN != (i64 = SDO_read(&SDO, ID))) fn(i64); else WARNX(e);}while(0)
-    getSDOe(ERRSTATE, chkerr, "Can't get error status");
-    getSDOe(DEVSTATUS, chkstat, "Can't get device status");
 
+#define Mesg(...)
+//#define Mesg(...)    green(__VA_ARGS__)
+    //double d0 = dtime();
+
+    getSDOe(ERRSTATE, chkerr, "Can't get error status");
+    Mesg("ERRSTATE: %g\n", dtime() - d0);
+    getSDOe(DEVSTATUS, chkstat, "Can't get device status");
+    Mesg("DEVSTATUS: %g\n", dtime() - d0);
     if(GP->parsefile){
         green("Try to parse %s and send SDOs to device\n", GP->parsefile);
         parse_data_file(GP->parsefile, GP->NodeID);
+        Mesg("parse_data_file: %g\n", dtime() - d0);
     }
 
     if(GP->showpars){
         showAllPars();
+        Mesg("showAllPars: %g\n", dtime() - d0);
+    }
+
+    if(GP->enableESW){
+        if(SDO_write(&EXTENABLE, ID, 3)){
+            WARNX("Error when trying to enable limit switches");
+            if(GP->absmove || GP->relmove) signals(-1);
+        }
+        Mesg("EXTENABLE: %g\n", dtime() - d0);
     }
 
     getSDOe(MICROSTEPS, setusteps, "Can't get microstepping");
+    Mesg("MICROSTEPS: %g\n", dtime() - d0);
     if(GP->zeropos){
         if(SDO_write(&POSITION, ID, 0))
             ERRX("Can't clear position counter");
+            Mesg("POSITION: %g\n", dtime() - d0);
     }
     if(INT64_MIN != (i64 = SDO_read(&POSITION, ID)))
         green("CURPOS=%d\n", (int)i64/microstepping);
-    else ERRX("Can't read current position");
+    else WARNX("Can't read current position");
+    Mesg("CURPOS: %g\n", dtime() - d0);
     getSDOe(MAXSPEED, setmaxspd, "Can't read max speed");
+    Mesg("MAXSPEED: %g\n", dtime() - d0);
     getSDOw(GPIOVAL, gpioval, "Can't read GPIO values");
+    Mesg("GPIOVAL: %g\n", dtime() - d0);
 
     if(GP->disable){
         if(SDO_write(&ENABLE, ID, 0)) ERRX("Can't disable motor");
+        Mesg("DISABLE: %g\n", dtime() - d0);
     }
 
     if(INT64_MIN != (i64 = SDO_read(&ENABLE, ID))){
         if(i64) green("ENABLE=1\n");
         else red("ENABLE=0\n");
+        Mesg("Status: %g\n", dtime() - d0);
     }
 
     if(GP->stop){
         if(SDO_write(&STOP, ID, 1)) ERRX("Can't stop motor");
+        Mesg("STOP: %g\n", dtime() - d0);
     }
 
     if(GP->absmove != INT_MIN){
         SDO_write(&ENABLE, ID, 1);
         if(SDO_write(&ABSSTEPS, ID, GP->absmove*microstepping))
             ERRX("Can't move to absolute position %d", GP->absmove);
+        Mesg("AbsMove: %g\n", dtime() - d0);
     }
     if(GP->relmove != INT_MIN && GP->relmove){
         uint8_t dir = 1;
@@ -236,6 +260,7 @@ int main(int argc, char *argv[]){
         DBG("i64=%ld, dir=%d", i64, dir);
         if(SDO_write(&RELSTEPS, ID, GP->relmove*microstepping))
             ERRX("Can't move to relative position %d", GP->relmove);
+        Mesg("RelMove: %g\n", dtime() - d0);
     }
 #undef getSDOe