diff --git a/BTA_dome_modbus/Makefile b/BTA_dome_modbus/Makefile
index e749ad3..c51e58f 100644
--- a/BTA_dome_modbus/Makefile
+++ b/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))
diff --git a/BTA_dome_modbus/client.c b/BTA_dome_modbus/client.c
index b22172f..39817a7 100644
--- a/BTA_dome_modbus/client.c
+++ b/BTA_dome_modbus/client.c
@@ -16,10 +16,51 @@
* along with this program. If not, see .
*/
+#include
+#include
+#include
#include
#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){
diff --git a/BTA_dome_modbus/cmdlnopts.c b/BTA_dome_modbus/cmdlnopts.c
index 11b8dbb..fc05340 100644
--- a/BTA_dome_modbus/cmdlnopts.c
+++ b/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
};
diff --git a/BTA_dome_modbus/cmdlnopts.h b/BTA_dome_modbus/cmdlnopts.h
index 658fd18..50a7965 100644
--- a/BTA_dome_modbus/cmdlnopts.h
+++ b/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
diff --git a/BTA_dome_modbus/daemon.c b/BTA_dome_modbus/daemon.c
index aab7dfd..2ead888 100644
--- a/BTA_dome_modbus/daemon.c
+++ b/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();
}
diff --git a/BTA_dome_modbus/daemon.h b/BTA_dome_modbus/daemon.h
index 7ab62c4..696a41e 100644
--- a/BTA_dome_modbus/daemon.h
+++ b/BTA_dome_modbus/daemon.h
@@ -18,5 +18,7 @@
#pragma once
+extern int isrunning;
+
int start_daemon();
diff --git a/BTA_dome_modbus/handlers_list.h b/BTA_dome_modbus/handlers_list.h
new file mode 100644
index 0000000..ec20786
--- /dev/null
+++ b/BTA_dome_modbus/handlers_list.h
@@ -0,0 +1,35 @@
+/*
+ * This file is part of the sslsosk project.
+ * Copyright 2026 Edward V. Emelianov .
+ *
+ * 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 .
+ */
+
+#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") \*/
diff --git a/BTA_dome_modbus/motors.c b/BTA_dome_modbus/motors.c
new file mode 100644
index 0000000..7da4fcf
--- /dev/null
+++ b/BTA_dome_modbus/motors.c
@@ -0,0 +1,100 @@
+/*
+ * This file is part of the sslsosk project.
+ * Copyright 2026 Edward V. Emelianov .
+ *
+ * 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 .
+ */
+
+#include
+#include
+
+#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;
+}
diff --git a/BTA_dome_modbus/motors.h b/BTA_dome_modbus/motors.h
new file mode 100644
index 0000000..2c6bc53
--- /dev/null
+++ b/BTA_dome_modbus/motors.h
@@ -0,0 +1,59 @@
+/*
+ * This file is part of the sslsosk project.
+ * Copyright 2026 Edward V. Emelianov .
+ *
+ * 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 .
+ */
+
+#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();
diff --git a/BTA_dome_modbus/server.c b/BTA_dome_modbus/server.c
index b1cece4..844e846 100644
--- a/BTA_dome_modbus/server.c
+++ b/BTA_dome_modbus/server.c
@@ -19,20 +19,87 @@
#include
#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;
}
diff --git a/BTA_dome_modbus/sslsock.c b/BTA_dome_modbus/sslsock.c
index ed6bae1..a208184 100644
--- a/BTA_dome_modbus/sslsock.c
+++ b/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);
}
diff --git a/BTA_dome_modbus/sslsock.h b/BTA_dome_modbus/sslsock.h
index c72c4d6..4b5fc31 100644
--- a/BTA_dome_modbus/sslsock.h
+++ b/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);
diff --git a/BTA_dome_modbus/sslsosk.files b/BTA_dome_modbus/sslsosk.files
index c57f565..1e686e7 100644
--- a/BTA_dome_modbus/sslsosk.files
+++ b/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