nxt

BTA_dome_modbus/Makefile

@@ -8,7 +8,7 @@ SOBJDIR := mkserver
 COBJDIR := mkclient
 CFLAGS += -O2 -Wall -Wextra -Wno-trampolines -pthread
 COMMSRCS := sslsock.c daemon.c cmdlnopts.c main.c
-SSRC := server.c $(COMMSRCS)
+SSRC := server.c motors.c $(COMMSRCS)
 CSRC := client.c bta_shdata.c $(COMMSRCS)
 SOBJS := $(addprefix $(SOBJDIR)/, $(SSRC:%.c=%.o))
 COBJS := $(addprefix $(COBJDIR)/, $(CSRC:%.c=%.o))

BTA_dome_modbus/client.c

@@ -16,10 +16,51 @@
  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#include <inttypes.h>
+#include <float.h>
+#include <signal.h>
 #include <usefull_macros.h>
 
 #include "bta_shdata.h"
 #include "client.h"
+#include "cmdlnopts.h"
+#include "daemon.h" // isrunning
+#include "handlers_list.h"
+#include "motors.h" // motors amount, motor_state_t
+#include "sslsock.h"
+
+
+#define NEW_HANDLER(name, unused)  \
+static const char* cmd_ ## name = STR(name);
+HANDLERS_LIST()
+#undef NEW_HANDLER
+
+static const char *Ecodes[RESULT_SILENCE] = {
+    [RESULT_OK] = "OK",
+    [RESULT_BADVAL] = "BADVAL",
+    [RESULT_BADKEY] = "BADKEY",
+    [RESULT_FAIL] = "FAIL",
+};
+
+typedef enum{
+    ARG_TYPE_INT,
+    ARG_TYPE_DOUBLE
+} arg_type_t;
+
+typedef struct{
+    union{
+        double d;
+        int64_t i;
+    };
+    arg_type_t type;
+} value_t;
+
+// setter: when setspeed command sent successfully
+static double set_speed = 0.;
+// common state - by last motor polled
+static motor_state_t CommonState = {0};
+// state of each motor
+static motor_state_t MotorState[MOTORS_AMOUNT] = {0};
 
 static int SSL_nbread(SSL *ssl, char *buf, int bufsz){
     struct pollfd fds = {0};
@@ -32,14 +73,252 @@ static int SSL_nbread(SSL *ssl, char *buf, int bufsz){
         return 0;
     }
     if(fds.revents == POLLIN){
-//        DBG("Got info in fd #%d", fd);
+        //        DBG("Got info in fd #%d", fd);
         int l = read_string(ssl, buf, bufsz);
-//        DBG("read %d bytes", l);
+        //        DBG("read %d bytes", l);
         return l;
     }
     return 0;
 }
 
+
+/**
+ * @brief send_motor_command - send motor command and parse answer
+ * @param ssl - ssl
+ * @param cmd - command to send
+ * @param setter - command setter (or NULL for getter)
+ * @param getter - pointer do value changed by getter (or NULL for setter)
+ * @return server's answer (or OK for successfull getters)
+ */
+static sl_sock_hresult_e send_motor_command(SSL *ssl, const char *cmd, value_t *setter, value_t *getter){
+    char buf[IOBUF_LEN];
+    if(!ssl || !cmd) return RESULT_FAIL;
+    int l;
+    if(setter){
+        if(setter->type == ARG_TYPE_INT){
+            l = snprintf(buf, IOBUF_LEN-1, "%s=%" PRId64 "\n", cmd, setter->i);
+        }else{
+            l = snprintf(buf, IOBUF_LEN-1, "%s=%g\n", cmd, setter->d);
+        }
+    }else l = snprintf(buf, IOBUF_LEN-1, "%s\n", cmd);
+    buf[l] = 0;
+    DBG("Send to server: %s", buf);
+    SSL_write(ssl, buf, l);
+    double t0 = sl_dtime();
+    while(sl_dtime() - t0 < G.acc_timeout){
+        l = SSL_nbread(ssl, buf, IOBUF_LEN-1);
+        if(l == 0) continue;
+        else if(l < 0){
+            LOGWARN("Server disconnected or other error");
+            ERRX("Disconnected");
+        }
+        buf[l] = 0;
+        DBG("Received: \"%s\"\n", buf);
+        // parser
+        char key[SL_KEY_LEN] = {0}, val[SL_VAL_LEN] = {0};
+        int got = sl_get_keyval(buf, key, val);
+        DBG("got=%d, key=%s, val=%s", got, key, val);
+        if(got == 0){
+            DBG("Empty answer");
+            continue;
+        }
+        if((!setter && got == 1) || (setter && got == 2)){ // wrong answer
+            DBG("wrong answer");
+            continue;
+        }
+        if(setter){ // check errcode in answer
+            if(got == 2){
+                DBG("Getter answer");
+                continue;
+            }
+            for(int i = 0; i < RESULT_SILENCE; ++i){
+                DBG("compare '%s' and '%s'", key, Ecodes[i]);
+                if(0 == strcmp(key, Ecodes[i])){
+                    DBG("Found errcode for '%s': %d", key, i);
+                    return i; // found
+                }
+            }
+        }else{ // check "cmd = val"
+            if(got == 1) continue;
+            if(strcmp(cmd, key)) continue;
+            if(getter){
+                if(getter->type == ARG_TYPE_INT){
+                    long long ll;
+                    if(!sl_str2ll(&ll, val)) continue;
+                    getter->i = (int64_t) ll;
+                }else{
+                    double d;
+                    if(!sl_str2d(&d, val)) continue;
+                    getter->d = d;
+                }
+            }
+            return RESULT_OK;
+        }
+    }
+    return RESULT_FAIL;
+}
+
+// emergency stop motors
+static void stop_all(SSL *ssl){
+    int ntries = 0;
+    value_t speed = {.type = ARG_TYPE_DOUBLE, .d = 0.};
+    while(ntries < 5){
+        if(RESULT_OK == send_motor_command(ssl, cmd_speed, &speed, NULL)) break;
+        ++ntries;
+        usleep(100000);
+    }
+}
+
+/**
+ * @brief check_motor - get status etc of motor number `motno`
+ * @param ssl - ssl
+ * @param motno - motor number (iterates on success)
+ * @return FALSE if some step failed
+ */
+static int check_motor(SSL *ssl, int motno){
+    char msg[128];
+    if(!ssl || motno < 0 || motno >= MOTORS_AMOUNT) return FALSE;
+    value_t Ival = {.type = ARG_TYPE_INT, .i = motno};
+    value_t Dval = {.type = ARG_TYPE_DOUBLE};
+    if(RESULT_OK != send_motor_command(ssl, cmd_motnum, &Ival, NULL)) return FALSE;
+    if(RESULT_OK != send_motor_command(ssl, cmd_motstatus, NULL, &Ival)) return FALSE;
+    MotorState[motno].status = (int) Ival.i;
+    if(RESULT_OK != send_motor_command(ssl, cmd_motspeed, NULL, &Dval)) return FALSE;
+    MotorState[motno].speed = Dval.d;
+    if(RESULT_OK != send_motor_command(ssl, cmd_motcurrent, NULL, &Dval)) return FALSE;
+    MotorState[motno].current = Dval.d;
+    // now set common state as mean of all
+    if(motno != MOTORS_AMOUNT - 1) return TRUE;
+    int status = 0, N = 0; // mean status is largest
+    double speed = 0., current = 0.;
+    for(int i = 0; i < MOTORS_AMOUNT; ++i){
+        int s = MotorState[i].status;
+        if(s == MOT_OFF) continue;
+        if(status < s) status = s;
+        speed += MotorState[i].speed;
+        current += MotorState[i].current;
+        ++N;
+    }
+    if(N){
+        speed /= (double)N;
+        current /= (double)N;
+    }
+    if(status == MOT_OFF && CommonState.status != MOT_OFF){
+        *msg = MesgFault;
+        sprintf(msg+1, "Dome: All motors are Off!\n");
+        SendMessage(msg);
+    }else if(status != MOT_OFF && CommonState.status == MOT_OFF){
+        *msg = MesgFault;
+        sprintf(msg+1, "Dome: Start motors!\n");
+        SendMessage(msg);
+    }
+    CommonState.status = status;
+    CommonState.speed = speed;
+    CommonState.current = current;
+    return TRUE;
+}
+
+// check for speed change and send given command to server
+static void chk_dome_speed(SSL *ssl){
+    static int old_state = D_Off;
+    int new_state = Dome_Speed;
+    double tlast = 0.;
+    if(new_state == old_state){
+        if(sl_dtime() - tlast < G.speedchk_interval) return;
+        if(CommonState.speed == set_speed){
+            tlast = sl_dtime();
+            return;
+        }
+    }
+    double new_speed = 0.;
+    switch(new_state){
+    case D_Lplus:
+        new_speed = LSpeed;
+        break;
+    case D_Lminus:
+        new_speed = -LSpeed;
+        break;
+    case D_Mplus:
+        new_speed = MSpeed;
+        break;
+    case D_Mminus:
+        new_speed = -MSpeed;
+        break;
+    case D_Hplus:
+        new_speed = HSpeed;
+        break;
+    case D_Hminus:
+        new_speed = -HSpeed;
+        break;
+    default: // stop
+        break;
+    }
+    value_t Dval = {.type = ARG_TYPE_DOUBLE, .d = new_speed};
+    if(RESULT_OK == send_motor_command(ssl, cmd_speed, &Dval, NULL)){
+        if(RESULT_OK == send_motor_command(ssl, cmd_speed, NULL, &Dval) && fabs(Dval.d - new_speed) <= FLT_EPSILON){
+            old_state = new_state; // all OK, command in work
+            set_speed = new_speed;
+            tlast = sl_dtime();
+        }
+    }
+}
+
+// SHM parser; return FALSE if SHM is in erroreous state
+static int process_system(SSL *ssl){
+    static double t0 = 0., last_mtime = 0.;
+    static int curMotNo = 0; // current motor number (we'll scan all 10 motors by one)
+    double curtime = sl_dtime();
+    if(t0 < 1.){ // first run
+        t0 = curtime;
+        last_mtime = M_time;
+        return TRUE;
+    }
+    // check time sync
+    static int brokenshm = FALSE;
+    if(M_time - last_mtime + G.T_sync_lost > curtime - t0 || !check_shm_block(&sdat)){ // broken SHM
+        if(!brokenshm){
+            LOGERR("Stalled or broken SHM block");
+            brokenshm = TRUE;
+        }
+        return FALSE;
+    }else brokenshm = FALSE;
+    static int modelused = FALSE;
+    if(UseModel == FullModel){ // model
+        if(!modelused){
+            LOGWARN("Server is in model mode");
+            modelused = TRUE;
+            stop_all(ssl);
+        }
+        return TRUE;
+    }else modelused = FALSE;
+    static int serverisdead = FALSE;
+    if(ServPID <= 0 || kill(ServPID, 0) < 0){ // dead server
+        if(!serverisdead){
+            LOGERR("Main server is dead");
+            serverisdead = TRUE;
+        }
+        return FALSE;
+    }else serverisdead = FALSE;
+    if(check_motor(ssl, curMotNo)){
+        // TODO: check state for errors
+        if(++curMotNo >= MOTORS_AMOUNT) curMotNo = 0;
+    }
+    if(!D_Locked && Dome_State != D_Off) chk_dome_speed(ssl);
+#if 0
+    if(curtime - t0 > 3.){
+        sprintf(buf, "help\n");
+        SSL_write(ssl, buf, strlen(buf));
+        /*DBG("OLD: %g", val_Hmd);
+            val_Hmd = 55. + drand48() * 15.;
+            DBG("New: %g", val_Hmd);*/
+        t0 = curtime;
+    }
+#endif
+    ;
+    return FALSE;
+}
+
+/*
 static char *time_asc(double t){
     static char buf[128];
     int h, m;
@@ -52,14 +331,15 @@ static char *time_asc(double t){
     snprintf(buf, 127, "%d:%02d:%04.1f", h,m,s);
     return buf;
 }
-
+*/
 
 void clientproc(SSL_CTX *ctx, int fd){
     FNAME();
     SSL *ssl;
     char buf[1024];
-    char acClientRequest[1024] = {0};
     int bytes;
+    sdat.mode |= 0200; // allow W
+    sdat.atflag = 0; // clear SHM_RDONLY
     if(!get_shm_block(&sdat, ClientSide)){
         LOGERR("Can't get SHM block");
         ERRX("Can't get SHM block");
@@ -76,17 +356,11 @@ void clientproc(SSL_CTX *ctx, int fd){
         LOGERR("Can't make socket nonblocking");
         ERRX("ioctl()");
     }
-    double t0 = sl_dtime();
-    while(1){
-        if(sl_dtime() - t0 > 3.){
-            if(!check_shm_block(&sdat)){
-                LOGERR("Broken SHM block");
-                ERRX("Broken SHM block");
-            }
-            sprintf(acClientRequest, "UTC: %s\n", time_asc(M_time));
-            SSL_write(ssl, acClientRequest, strlen(acClientRequest));
-            //val_Hmd = 55. + drand48() * 15.;
-            t0 = sl_dtime();
+    while(isrunning){
+        if(!process_system(ssl)){
+            stop_all(ssl);
+            sleep(5);
+            break;
         }
         bytes = SSL_nbread(ssl, buf, sizeof(buf));
         if(bytes > 0){

BTA_dome_modbus/cmdlnopts.c

@@ -46,6 +46,8 @@ glob_pars G = {
     .key = DEFKEY,
     .ca = DEFCA,
     .acc_timeout = 1.,
+    .T_sync_lost = 5.,
+    .speedchk_interval = 10.,
 };
 
 /*
@@ -62,11 +64,13 @@ static sl_option_t cmdlnopts[] = {
     {"port",    NEED_ARG,   NULL,   'p',    arg_string, APTR(&G.port),      _("port to open (default: " DEFAULT_PORT ")")},
     {"verbose", NO_ARGS,    NULL,   'v',    arg_none,   APTR(&G.verbose),   _("increase log verbose level (default: LOG_WARN)")},
     {"ca",      NEED_ARG,   NULL,   'a',    arg_string, APTR(&G.ca),        _("path to SSL ca - base cert (default:" DEFCA ")")},
+    {"timeout", NEED_ARG,   NULL,   't',    arg_double, APTR(&G.acc_timeout),_("network timeout, s (default: 1)")},
 #ifdef SERVER
-    {"timeout", NEED_ARG,   NULL,   't',    arg_double, APTR(&G.acc_timeout),_("server's accept timeout, s")},
 #endif
 #ifdef CLIENT
-    {"server",  NEED_ARG,   NULL,   's',    arg_string, APTR(&G.serverhost),  _("server IP address or name")},
+    {"mottmout",NEED_ARG,   NULL,   'M',    arg_double, APTR(&G.speedchk_interval), _("interval of motor's speed checking, s (default: 10)")},
+    {"server",  NEED_ARG,   NULL,   's',    arg_string, APTR(&G.serverhost), _("server node, IP:port")},
+    {"lostsync",NEED_ARG,   NULL,   'L',    arg_double, APTR(&G.T_sync_lost),_("\"lost synchronization\" timeout, s (default: 5)")},
 #endif
    end_option
 };

BTA_dome_modbus/cmdlnopts.h

@@ -34,7 +34,9 @@
  * here are some typedef's for global data
  */
 typedef struct{
-    int acc_timeout;        // accept() timeout
+    double acc_timeout;     // network timeout, s
+    double T_sync_lost;     // "lost synchronization" timeout, s
+    double speedchk_interval;// interval of motors' speed checking (resend command if don't reach yet)
     char *pidfile;          // name of PID file
     char *logfile;          // logging to this file
     char *cert;             // sertificate

BTA_dome_modbus/daemon.c

@@ -29,12 +29,15 @@
 
 static pid_t childpid = -1;
 static double tstart = 0.; // time of fork()
+int isrunning = FALSE;
 
 void signals(int sig){
     int savelogs = (sl_dtime() - tstart > 30.) ? TRUE : FALSE;
-    if(childpid == 0){
+    if(childpid == 0){ // slave process
         if(savelogs) LOGWARN("Child killed with sig=%d", sig);
-        exit(sig); // slave process
+        isrunning = FALSE;
+        sleep(1); // allow graceful stop
+        exit(sig);
     }
     // master process
     if(sig){
@@ -106,7 +109,7 @@ int start_daemon(){
                 LOGMSG("Child %d works for %.1f %s", childpid, Tw, Tstr[idx]);
                 savelogs = TRUE;
             }
-            sleep(2); // wait a little before respawn
+            sleep(5); // wait a little before respawn
             tstart = sl_dtime();
         }else{ // slave
             prctl(PR_SET_PDEATHSIG, SIGTERM); // send SIGTERM to child when parent dies
@@ -114,6 +117,7 @@ int start_daemon(){
         }
     }
 #endif
+    isrunning = TRUE;
     // parent should never reach this part of code
     return open_socket();
 }

BTA_dome_modbus/daemon.h

@@ -18,5 +18,7 @@
 
 #pragma once
 
+extern int isrunning;
+
 int start_daemon();
 

BTA_dome_modbus/handlers_list.h

@@ -0,0 +1,35 @@
+/*
+ * This file is part of the sslsosk project.
+ * Copyright 2026 Edward V. Emelianov <edward.emelianoff@gmail.com>.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#ifndef STR
+#define SSS_(x)   #x
+#define STR(x)  SSS_(x)
+#endif
+
+// Handlers in this list MUST be in sortered order (by name)!!!
+#define HANDLERS_LIST() \
+NEW_HANDLER(motcurrent, "motor current") \
+NEW_HANDLER(motnum, "active motor number for status requests") \
+NEW_HANDLER(motspeed, "motor speed") \
+NEW_HANDLER(motstatus, "motor status") \
+NEW_HANDLER(speed, "speed setter") \
+
+/*NEW_HANDLER(current, "current setter") \*/
+/*NEW_HANDLER(relay, "relay command") \*/

BTA_dome_modbus/motors.c

@@ -0,0 +1,100 @@
+/*
+ * This file is part of the sslsosk project.
+ * Copyright 2026 Edward V. Emelianov <edward.emelianoff@gmail.com>.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <math.h>
+#include <usefull_macros.h>
+
+#include "motors.h"
+
+#if 0
+ðÒÏÔÏËÏÌ (s - ÓÅÔÔÅÒ, g - getteer):
+relay=xxx - (sg) ËÏÍÁÎÄÁ ÒÅÌÅ
+motnum=xx - (sg) ÐÏÌÕÞÉÔØ Ó×ÅÄÅÎÉÑ Ï Ä×ÉÇÁÔÅÌÅ Ó ÎÏÍÅÒÏÍ ÈÈ
+(ÔÅËÕÝÅÅ ÓÏÓÔÏÑÎÉÅ):
+motstatus=xx - (g) ÓÏÓÔÏÑÎÉÅ ÍÏÔÏÒÁ
+motspeed=xx - (g) ÒÅÁÌØÎÁÑ ÓËÏÒÏÓÔØ
+motcurrent=xx - (g) ÒÅÁÌØÎÙÊ ÔÏË
+(ÄÌÑ ×ÓÅÈ ÍÏÔÏÒÏ×):
+speed=xx - (sg) ÕÓÔÁ×ËÁ ÓËÏÒÏÓÔÉ
+current=xx - (sg) ÕÓÔÁ×ËÁ ÔÏËÁ
+#endif
+
+// set points
+static double currentSet = 0., speedSet = 0.;
+// current motor for status etc. getters
+static int motindex = 0;
+
+// stop all
+void motors_stop(){
+    ;
+}
+
+// close modbus connection
+void modbus_close(){
+    ;
+}
+
+// current setpoint getter
+double motors_get_curntsetpoint(){
+    return currentSet;
+}
+// set setpoint of current
+int motors_set_curntsetpoint(double val){
+    if(val < 0. || val > MAX_CURRENT) return FALSE;
+    // do something
+    return TRUE;
+}
+
+// get setpoint of speed
+double motors_get_speedsetpoint(){
+    return speedSet;
+}
+// set setpoint of speed
+int motors_set_speedsetpoint(double val){
+    double absval = fabs(val);
+    if(absval > MAX_SPEED) return FALSE;
+    // do something
+    return TRUE;
+}
+
+// get number of active motor
+int motors_get_activenum(){ return motindex; }
+// set number of active motor
+int motors_set_activenum(int N){
+    if(N < 1 || N > MOTORS_AMOUNT) return FALSE;
+    motindex = N;
+    return TRUE;
+}
+
+// get current value for active motor
+int motors_get_actcurrent(double *val){
+    if(val) *val = 0.;
+    return TRUE;
+}
+
+// get speed for active motor
+int motors_get_actspeed(double *val){
+    if(val) *val = 0.;
+    return TRUE;
+}
+
+// get status for active motor
+int motors_get_actstatus(int *val){
+    if(val) *val = 0;
+    return TRUE;
+}

BTA_dome_modbus/motors.h

@@ -0,0 +1,59 @@
+/*
+ * This file is part of the sslsosk project.
+ * Copyright 2026 Edward V. Emelianov <edward.emelianoff@gmail.com>.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#define MAX_SPEED       (700.)
+#define MAX_CURRENT     (15.)
+#define MOTORS_AMOUNT   (10)
+// low, medium and high speeds
+#define LSpeed    71
+#define MSpeed    350
+#define HSpeed    610
+
+// ID of first motor minus 1
+#define START_ID        (0)
+// ID of motor (n=1..MOTORS_AMOUNT)
+#define MOTOR_ID(n)     (n + START_ID)
+
+// motors' status
+enum{
+    MOT_OFF,
+    MOT_SLEEP,
+    MOT_RUN,
+    MOT_ERROR
+};
+
+typedef struct{
+    int status;
+    double speed;
+    double current;
+} motor_state_t;
+
+void motors_stop();
+double motors_get_curntsetpoint();
+int motors_set_curntsetpoint(double val);
+double motors_get_speedsetpoint();
+int motors_set_speedsetpoint(double val);
+int motors_get_activenum();
+int motors_set_activenum(int N);
+int motors_get_actcurrent(double *val);
+int motors_get_actspeed(double *val);
+int motors_get_actstatus(int *val);
+
+void modbus_close();

BTA_dome_modbus/server.c

@@ -19,20 +19,87 @@
 #include <usefull_macros.h>
 
 #include "cmdlnopts.h"
+#include "daemon.h" // isrunning
+#include "handlers_list.h"
+#include "motors.h"
 #include "server.h"
 
+// handlers: `index` - command index in list, `value` - setter's value or getter's answer
+typedef sl_sock_hresult_e (*handler_t)(int index, char value[SL_VAL_LEN]);
+
+// struct for setters/getters
+typedef struct{
+    const char *command;
+    handler_t handler;
+    const char *helpstring;
+} command_t;
+
 static const char *maxcl = "Max client number reached, connect later\n";
 static const char *sslerr = "SSL error occured\n";
 
+// declaration of handlers
+#define NEW_HANDLER(name, unused)  \
+static sl_sock_hresult_e name ## _handler(int, char[SL_VAL_LEN]); // static const char* cmd_ ## name = STR(name);
+HANDLERS_LIST()
+#undef NEW_HANDLER
+
+// commands list
+#define NEW_HANDLER(name, help)  \
+    { STR(name), name ## _handler, help },
+command_t command_list[] = {
+    HANDLERS_LIST()
+};
+#undef NEW_HANDLER
+// index of command like `current_idx`
+#define NEW_HANDLER(name, help)  \
+    name ## _idx,
+enum{
+    HANDLERS_LIST()
+};
+#undef NEW_HANDLER
+
+#define HANDLERS_AMOUNT (sizeof(command_list) / sizeof(command_t))
+#define ISSETTER(x) (0 != x[0])
+
+// search handler by name
+static int search_handler(const char *);
+
 // return 0 if client disconnected
 static int handle_connection(SSL *ssl){
-    char buf[1024];
-    int r = read_string(ssl, buf, 1024);
+    char buf[IOBUF_LEN], key[SL_KEY_LEN], val[SL_VAL_LEN];
+    int r = read_string(ssl, buf, IOBUF_LEN);
     if(r < 0) return 0;
     int sd = SSL_get_fd(ssl);
-    printf("Client %d msg: \"%s\"\n", sd, buf);
+    DBG("Client %d msg: \"%s\"\n", sd, buf);
     LOGDBG("fd=%d, message=%s", sd, buf);
-    snprintf(buf, 1024, "Hello, your FD=%d\n", sd);
+    int got = sl_get_keyval(buf, key, val);
+    if(got == 0){
+        DBG("Comment or empty string");
+        return 1; // empty string
+    }
+    int h_idx = search_handler(key);
+    sl_sock_hresult_e result = RESULT_BADKEY;
+    if(-1 != h_idx){
+        if(got == 1){
+            DBG("getter #%d", h_idx);
+            val[0] = 0; // getter
+        }else DBG("setter #%d", h_idx);
+        result = command_list[h_idx].handler(h_idx, val);
+        DBG("result: %d", result);
+    }else{
+        DBG("Command not found or help?");
+        // check if user asks for help
+        if(0 == strcmp(key, "help")){
+            for(size_t i = 0; i < HANDLERS_AMOUNT; ++i){
+                snprintf(buf,  IOBUF_LEN-1, "%s: %s\n", command_list[i].command, command_list[i].helpstring);
+                SSL_write(ssl, buf, strlen(buf));
+            }
+            return 1;
+        }
+    }
+    // now `val` is an answer or error code
+    if(result != RESULT_SILENCE) snprintf(buf, IOBUF_LEN-1, "%s\n", sl_sock_hresult2str(result));
+    else snprintf(buf, IOBUF_LEN-1, "%s=%s\n", key, val);
     SSL_write(ssl, buf, strlen(buf));
     return 1;
 }
@@ -60,8 +127,6 @@ static int timeouted_sslaccept(SSL *ssl){
 }
 
 void serverproc(SSL_CTX *ctx, int fd){
-    char buf[64];
-    int P = 0;
     int enable = 1;
     if(ioctl(fd, FIONBIO, (void *)&enable) < 0){
         LOGERR("Can't make socket nonblocking");
@@ -73,9 +138,11 @@ void serverproc(SSL_CTX *ctx, int fd){
     poll_set[0].fd = fd;
     poll_set[0].events = POLLIN;
     SSL *ssls[BACKLOG+1] = {0}; // !!! start from 1 - like in poll_set !!!
-    double t0 = sl_dtime(), tstart = t0;
-    while(1){
-        double tnow = sl_dtime();
+    //double t0 = sl_dtime(), tstart = t0;
+    //char buf[64];
+    //int P = 0;
+    while(isrunning){
+        /*double tnow = sl_dtime();
         if(tnow - t0 > 5. && nfd > 1){ // broadcasting message
             //DBG("send ping");
             snprintf(buf, 63, "ping #%d; t=%g\n", ++P, tnow - tstart);
@@ -85,7 +152,7 @@ void serverproc(SSL_CTX *ctx, int fd){
                 SSL_write(ssls[i], buf, l);
             }
             t0 = tnow;
-        }
+        }*/
         poll(poll_set, nfd, 1); // max timeout - 1ms
         // check for accept()
         if(poll_set[0].revents & POLLIN){
@@ -98,6 +165,8 @@ void serverproc(SSL_CTX *ctx, int fd){
                 LOGWARN("Max amount of connections: disconnect fd=%d", client);
                 WARNX("Limit of connections reached");
                 send(client, maxcl, sizeof(maxcl)-1, MSG_NOSIGNAL);
+                shutdown(client, SHUT_WR);
+                usleep(50000); // we can't allow client to block us
                 close(client);
             }else{
                 DBG("New ssl");
@@ -136,4 +205,92 @@ void serverproc(SSL_CTX *ctx, int fd){
             }
         }
     }
+    for(int i = 0; i < nfd; ++i) SSL_free(ssls[i]);
+    motors_stop();
+    modbus_close();
+}
+
+/****************** Protocol handlers (return 0 in case of success or error code >0 if failed) ******************/
+// key - keyword (command name), value - i/o buffer (value[0]==0 for getters)
+/*
+sl_sock_hresult_e current_handler(int _U_ index, char _U_ value[SL_VAL_LEN]){
+    double D;
+    if(ISSETTER(value)){
+        if(!sl_str2d(&D, value) || !motors_set_curntsetpoint(D)) return RESULT_BADVAL;
+        return RESULT_OK;
+    }
+    snprintf(value,  SL_VAL_LEN-1, "%.3f", motors_get_curntsetpoint());
+    return RESULT_SILENCE;
+}*/
+
+sl_sock_hresult_e motcurrent_handler(int _U_ index, char _U_ value[SL_VAL_LEN]){
+    if(ISSETTER(value)) return RESULT_BADVAL; // only getter
+    double D;
+    if(!motors_get_actcurrent(&D)) return RESULT_FAIL;
+    snprintf(value,  SL_VAL_LEN-1, "%.3f", D);
+    return RESULT_SILENCE;
+}
+
+sl_sock_hresult_e motnum_handler(int _U_ index, char _U_ value[SL_VAL_LEN]){
+    int I;
+    if(ISSETTER(value)){
+        if(!sl_str2i(&I, value) || !motors_set_activenum(I)) return RESULT_BADVAL;
+        return RESULT_OK;
+    }
+    snprintf(value, SL_VAL_LEN-1, "%d", motors_get_activenum());
+    return RESULT_SILENCE;
+}
+
+sl_sock_hresult_e motspeed_handler(int _U_ index, char _U_ value[SL_VAL_LEN]){
+    if(ISSETTER(value)) return RESULT_BADVAL; // only getter
+    double D;
+    if(!motors_get_actspeed(&D)) return RESULT_FAIL;
+    snprintf(value,  SL_VAL_LEN-1, "%.3f", D);
+    return RESULT_SILENCE;
+}
+
+sl_sock_hresult_e motstatus_handler(int _U_ index, char _U_ value[SL_VAL_LEN]){
+    if(ISSETTER(value)) return RESULT_BADVAL; // only getter
+    int I;
+    if(!motors_get_actstatus(&I)) return RESULT_FAIL;
+    snprintf(value, SL_VAL_LEN-1, "%d", I);
+    return RESULT_SILENCE;
+}
+/*
+sl_sock_hresult_e relay_handler(int _U_ index, char _U_ value[SL_VAL_LEN]){
+    return RESULT_SILENCE;
+}*/
+
+sl_sock_hresult_e speed_handler(int _U_ index, char _U_ value[SL_VAL_LEN]){
+    double D;
+    if(ISSETTER(value)){
+        if(!sl_str2d(&D, value) || !motors_set_speedsetpoint(D)) return RESULT_BADVAL;
+        return RESULT_OK;
+    }
+    snprintf(value,  SL_VAL_LEN-1, "%.3f", motors_get_speedsetpoint());
+    return RESULT_SILENCE;
+}
+
+// binary search handler by name
+static int search_handler(const char *name){
+    int low = 0;
+    int high = HANDLERS_AMOUNT - 1;
+    int iter = 0;
+
+    while(low <= high){
+        ++iter;
+        int mid = low + (high - low) / 2;
+        // Compare the target string with the struct's string field
+        int res = strcmp(name, command_list[mid].command);
+        if(res == 0){
+            DBG("Found %s by %d iterations\n", name, iter);
+            return mid; // Target found, return index
+        }else if(res < 0){
+            high = mid - 1; // Target is smaller, search left half
+        }else{
+            low = mid + 1;  // Target is larger, search right half
+        }
+    }
+    DBG("%s not found by %d iterations\n", name, iter);
+    return -1;
 }

BTA_dome_modbus/sslsock.c

@@ -168,15 +168,20 @@ static int geterrcode(SSL *ssl, int errcode){
 int read_string(SSL *ssl, char *buf, int l){
     if(!ssl || l < 1) return 0;
     int bytes = SSL_peek(ssl, buf, l);
-    DBG("Peek: %d", bytes);
+    //DBG("Peek: %d", bytes);
     if(bytes < 1){ // nothing to read or error
         return geterrcode(ssl, bytes);
     }
-    if(bytes < l && buf[bytes-1] != '\n'){ // string not ready, no buffer overfull
-        return 0; // wait a rest of string
+    char *eol = strchr(buf, '\n');
+    if(!eol){ // not found: overflow or no enough symbols
+        if(bytes == l){ // oops: overflow, thow it out
+            SSL_read(ssl, buf, l);
+        }
+        return 0;
     }
+    l = eol - buf + 1;
     bytes = SSL_read(ssl, buf, l);
-    DBG("Read: %d", bytes);
+    //DBG("Read: %d", bytes);
     if(bytes < 1){ // error
         return geterrcode(ssl, bytes);
     }

BTA_dome_modbus/sslsock.h

@@ -37,5 +37,8 @@
 
 #define BACKLOG     10
 
+// length of string buffer for in/out argument `value` (for handlers)
+#define IOBUF_LEN   256
+
 int open_socket();
 int read_string(SSL *ssl, char *buf, int l);

BTA_dome_modbus/sslsosk.files

@@ -6,7 +6,10 @@ cmdlnopts.c
 cmdlnopts.h
 daemon.c
 daemon.h
+handlers_list.h
 main.c
+motors.c
+motors.h
 server.c
 server.h
 sslsock.c