From 71b0c9f5afe4e4b12551c47fa28d68e5f48d0793 Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Fri, 7 Aug 2026 17:01:40 +0300 Subject: [PATCH] nxt --- BTA_dome_modbus/Makefile | 2 +- BTA_dome_modbus/client.c | 304 ++++++++++++++++++++++++++++++-- BTA_dome_modbus/cmdlnopts.c | 8 +- BTA_dome_modbus/cmdlnopts.h | 4 +- BTA_dome_modbus/daemon.c | 10 +- BTA_dome_modbus/daemon.h | 2 + BTA_dome_modbus/handlers_list.h | 35 ++++ BTA_dome_modbus/motors.c | 100 +++++++++++ BTA_dome_modbus/motors.h | 59 +++++++ BTA_dome_modbus/server.c | 177 +++++++++++++++++-- BTA_dome_modbus/sslsock.c | 13 +- BTA_dome_modbus/sslsock.h | 3 + BTA_dome_modbus/sslsosk.files | 3 + 13 files changed, 684 insertions(+), 36 deletions(-) create mode 100644 BTA_dome_modbus/handlers_list.h create mode 100644 BTA_dome_modbus/motors.c create mode 100644 BTA_dome_modbus/motors.h 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