/* * geany_encoding=koi8-r * socket.c - socket IO * * Copyright 2018 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 2 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, write to the Free Software * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, * MA 02110-1301, USA. * */ #include "can_encoder.h" #include "HW_dependent.h" #include "usefull_macros.h" #include "socket.h" #include // addrinfo #include // inet_ntop #include #include // INT_xxx #include // fabs #include // pthread_kill #include // daemon #include // syscall #include "cmdlnopts.h" // glob_pars #define BUFLEN (1024) // Max amount of connections #define BACKLOG (30) extern glob_pars *G; /**************** COMMON FUNCTIONS ****************/ /** * wait for answer from socket * @param sock - socket fd * @return 0 in case of error or timeout, 1 in case of socket ready */ static int waittoread(int sock){ fd_set fds; struct timeval timeout; int rc; timeout.tv_sec = 1; // wait not more than 1 second timeout.tv_usec = 0; FD_ZERO(&fds); FD_SET(sock, &fds); do{ rc = select(sock+1, &fds, NULL, NULL, &timeout); if(rc < 0){ if(errno != EINTR){ WARN("select()"); return 0; } continue; } break; }while(1); if(FD_ISSET(sock, &fds)) return 1; return 0; } /**************** SERVER FUNCTIONS ****************/ // `canbus_mutex` used to exclude simultaneous CAN messages // `moving_mutex` used to block simultaneous attempts to move motor static pthread_mutex_t canbus_mutex = PTHREAD_MUTEX_INITIALIZER, moving_mutex = PTHREAD_MUTEX_INITIALIZER; bool emerg_stop = FALSE; /** * Send data over socket * @param sock - socket fd * @param webquery - ==1 if this is web query * @param buf - buffer with data (zero-terminated) * @return 1 if all OK */ static int send_data(int sock, int webquery, char *buf){ if(!buf) return 0; ssize_t L, Len = strlen(buf); //DBG("buf: %s, Len: %zd", buf, Len); if(Len < 1) return 0; char tbuf[BUFLEN]; // OK buffer ready, prepare to send it if(webquery){ L = snprintf(tbuf, BUFLEN, "HTTP/2.0 200 OK\r\n" "Access-Control-Allow-Origin: *\r\n" "Access-Control-Allow-Methods: GET, POST\r\n" "Access-Control-Allow-Credentials: true\r\n" "Content-type: text/plain\r\nContent-Length: %zd\r\n\r\n", Len); if(L < 0){ WARN("sprintf()"); return 0; } if(L != write(sock, tbuf, L)){ WARN("write"); return 0; } } if(Len != write(sock, buf, Len)){ WARN("write()"); return 0; } return 1; } // search a first word after needle without spaces static char* stringscan(char *str, char *needle){ char *a, *e; char *end = str + strlen(str); a = strstr(str, needle); if(!a) return NULL; a += strlen(needle); while (a < end && (*a == ' ' || *a == '\r' || *a == '\t' || *a == '\r')) a++; if(a >= end) return NULL; e = strchr(a, ' '); if(e) *e = 0; return a; } static uint8_t ismoving = 0; // ==1 when moving thread is active /** * @brief move_focus - separate thread moving focus to given position * @param targpos - target position */ static void *move_focus(void *targpos){ double pos = *((double*)targpos); DBG("MOVE FOCUS: %g", pos); pthread_mutex_lock(&canbus_mutex); ismoving = 1; // in any error case we should check end-switches and move out of them! if(move2pos(pos)) go_out_from_ESW(); ismoving = 0; putlog("Focus value: %.03f", curPos()); pthread_mutex_unlock(&moving_mutex); pthread_mutex_unlock(&canbus_mutex); pthread_exit(NULL); return NULL; } static const char *startmoving(double pos){ static double sp; if(pthread_mutex_trylock(&moving_mutex)) return S_ANS_MOVING; pthread_t m_thread; DBG("startmoving: %g", pos); sp = pos; if(pthread_create(&m_thread, NULL, move_focus, (void*) &sp)){ WARN("pthread_create()"); pthread_mutex_unlock(&moving_mutex); return S_ANS_ERR; }else{ DBG("Thread created, detouch"); pthread_detach(m_thread); // don't care about thread state } return S_ANS_OK; } /** * @brief ego, getoutESW - run go_out_from_ESW in a separate thread */ static void *ego(_U_ void *unused){ DBG("MOVE OUT FROM END-SWITCH"); pthread_mutex_lock(&canbus_mutex); go_out_from_ESW(); pthread_mutex_unlock(&canbus_mutex); pthread_mutex_unlock(&moving_mutex); pthread_exit(NULL); return NULL; } static void getoutESW(){ pthread_mutex_lock(&moving_mutex); pthread_t m_thread; if(pthread_create(&m_thread, NULL, ego, NULL)){ WARN("pthread_create()"); pthread_mutex_unlock(&moving_mutex); }else{ DBG("Thread created, detouch"); pthread_detach(m_thread); // don't care about thread state } } static void *handle_socket(void *asock){ #define getparam(x) (strncmp(found, x, sizeof(x)-1) == 0) int sock = *((int*)asock); int webquery = 0; // whether query is web or regular char buff[BUFLEN]; ssize_t rd; struct sockaddr_in peer; socklen_t peerlen = sizeof(peer); char *peerIP = NULL; if(getpeername(sock, &peer, &peerlen)){ WARN("getpeername() failed"); }else{ peerIP = strdup(inet_ntoa(peer.sin_addr)); } double t0 = dtime(); while(dtime() - t0 < SOCKET_TIMEOUT){ if(!waittoread(sock)){ // no data incoming //DBG("no incoming data"); continue; } if((rd = read(sock, buff, BUFLEN-1)) < 1){ //DBG("socket closed. Exit"); break; } //DBG("Got %zd bytes", rd); // add trailing zero to be on the safe side buff[rd] = 0; // now we should check what do user want char *got, *found = buff; if((got = stringscan(buff, "GET")) || (got = stringscan(buff, "POST"))){ // web query webquery = 1; char *slash = strchr(got, '/'); if(slash) found = slash + 1; // web query have format GET /some.resource } // here we can process user data //DBG("user send: %s%s\n", buff, webquery ? ", web" : ""); // empty request == focus request // TODO: add requests for min/max values (focus & speed) if(strlen(found) < 1 || getparam(S_CMD_FOCUS)){ //DBG("position request"); snprintf(buff, BUFLEN, "%.03f", curPos()); }else if(getparam(S_CMD_LIMITS)){ // send to user limit values snprintf(buff, BUFLEN, "focmin=%g\nfocmax=%g\nminspeed=%d\nmaxspeed=%d\n", FOCMIN_MM, FOCMAX_MM, MINSPEED, MAXSPEED); }else if(getparam(S_CMD_STOP)){ DBG("Stop request"); emerg_stop = TRUE; pthread_mutex_lock(&canbus_mutex); pthread_mutex_lock(&moving_mutex); if(stop()) sprintf(buff, S_ANS_ERR); else sprintf(buff, S_ANS_OK); emerg_stop = FALSE; putlog("%s: request to stop @ %.03f", peerIP, curPos()); pthread_mutex_unlock(&moving_mutex); pthread_mutex_unlock(&canbus_mutex); }else if(getparam(S_CMD_TARGSPEED)){ char *ch = strchr(found, '='); double spd; if(pthread_mutex_trylock(&moving_mutex)) sprintf(buff, S_ANS_MOVING); else{ pthread_mutex_lock(&canbus_mutex); if(!ch || !str2double(&spd, ch+1) || fabs(spd) < MINSPEED || fabs(spd) > MAXSPEED || movewconstspeed(spd)) sprintf(buff, S_ANS_ERR); else{ DBG("Move with constant speed %g request", spd); putlog("%s: move with speed %g, current pos.: %.03f", peerIP, spd, curPos()); sprintf(buff, S_ANS_OK); } pthread_mutex_unlock(&canbus_mutex); pthread_mutex_unlock(&moving_mutex); } }else if(getparam(S_CMD_GOTO)){ char *ch = strchr(found, '='); double pos; if(!ch || !str2double(&pos, ch+1) || pos < FOCMIN_MM || pos > FOCMAX_MM) sprintf(buff, S_ANS_ERR); else{ const char *ans = startmoving(pos); putlog("%s: move to %.03f, current pos.: %.03f", peerIP, pos, curPos()); addtolog("status: %s", ans); sprintf(buff, "%s", ans); DBG("Move to position %g request, status: %s", pos, ans); } }else if(getparam(S_CMD_STATUS)){ const char *msg = S_STATUS_ERROR; switch(get_status()){ case STAT_OK: if(ismoving) msg = S_STATUS_MOVING; else msg = S_STATUS_OK; break; case STAT_DAMAGE: msg = S_STATUS_DAMAGE; break; case STAT_ERROR: msg = S_STATUS_ERROR; break; case STAT_ESW: msg = S_STATUS_ESW; break; case STAT_GOFROMESW: msg = S_STATUS_GOFROMESW; break; case STAT_FORBIDDEN: msg = S_STATUS_FORBIDDEN; break; default: msg = "Unknown status"; } sprintf(buff, "%s", msg); }else sprintf(buff, S_ANS_ERR); if(!send_data(sock, webquery, buff)){ WARNX("can't send data, some error occured"); } } FREE(peerIP); close(sock); pthread_exit(NULL); return NULL; #undef getparam } // main socket server static void *server(void *asock){ int sock = *((int*)asock); if(listen(sock, BACKLOG) == -1){ //WARN("listen() failed"); return NULL; } while(1){ socklen_t size = sizeof(struct sockaddr_in); struct sockaddr_in their_addr; int newsock; if(!waittoread(sock)) continue; newsock = accept(sock, (struct sockaddr*)&their_addr, &size); if(newsock <= 0){ WARN("accept() failed"); continue; } struct sockaddr_in* pV4Addr = (struct sockaddr_in*)&their_addr; struct in_addr ipAddr = pV4Addr->sin_addr; char str[INET_ADDRSTRLEN]; inet_ntop(AF_INET, &ipAddr, str, INET_ADDRSTRLEN); //DBG("Got connection from %s", str); pthread_t handler_thread; if(pthread_create(&handler_thread, NULL, handle_socket, (void*) &newsock)){ WARN("pthread_create() failed"); }else{ //DBG("Thread created, detouch"); pthread_detach(handler_thread); // don't care about thread state } } putlog("UNREACHABLE CODE REACHED!"); } // refresh file with focus value static void subst_file(char *name){ if(!name) return; int l = strlen(name) + 7; char *aname = MALLOC(char, l); snprintf(aname, l, "%sXXXXXX", name); int fd = mkstemp(aname); if(fd < 0) goto ret; fchmod(fd, 0644); FILE *f = fdopen(fd, "w"); if(!f) goto ret; fprintf(f, "FOCUS = %.2f\n", curPos()); fclose(f); rename(aname, name); ret: FREE(aname); } // data gathering & socket management static void daemon_(int sock){ if(sock < 0) return; pthread_t sock_thread; double oldpos = curPos(); subst_file(G->focfilename); DBG("create server() thread"); if(pthread_create(&sock_thread, NULL, server, (void*) &sock)){ ERR("pthread_create() failed"); } do{ if(pthread_kill(sock_thread, 0) == ESRCH){ // died //WARNX("sockets thread died"); pthread_join(sock_thread, NULL); if(pthread_create(&sock_thread, NULL, server, (void*) &sock)){ ERR("new pthread_create() failed"); } } usleep(50000); // sleep a little or thread's won't be able to lock mutex // get current position if(!pthread_mutex_trylock(&canbus_mutex)){ getPos(NULL); sysstatus st = get_status(); pthread_mutex_unlock(&canbus_mutex); if(st != STAT_OK){ getoutESW(); } } if(G->focfilename && (fabs(oldpos - curPos()) > 0.01)){ // position changed -> change it in file oldpos = curPos(); subst_file(G->focfilename); } }while(1); putlog("daemon_(): UNREACHABLE CODE REACHED!"); } /** * Run daemon service */ void daemonize(const char *port){ int sock = -1; struct addrinfo hints, *res, *p; memset(&hints, 0, sizeof(hints)); hints.ai_family = AF_INET; hints.ai_socktype = SOCK_STREAM; hints.ai_flags = AI_PASSIVE; if(getaddrinfo(NULL, port, &hints, &res) != 0){ ERR("getaddrinfo failed"); } struct sockaddr_in *ia = (struct sockaddr_in*)res->ai_addr; char str[INET_ADDRSTRLEN]; inet_ntop(AF_INET, &(ia->sin_addr), str, INET_ADDRSTRLEN); DBG("canonname: %s, port: %u, addr: %s", res->ai_canonname, ntohs(ia->sin_port), str); // loop through all the results and bind to the first we can for(p = res; p != NULL; p = p->ai_next){ if((sock = socket(p->ai_family, p->ai_socktype, p->ai_protocol)) == -1){ continue; } int reuseaddr = 1; if(setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &reuseaddr, sizeof(int)) == -1){ ERR("setsockopt failed"); } if(bind(sock, p->ai_addr, p->ai_addrlen) == -1){ close(sock); continue; } break; // if we get here, we have a successfull connection } if(p == NULL){ // looped off the end of the list with no successful bind ERRX("failed to bind socket"); } freeaddrinfo(res); DBG("going to run daemon_()"); daemon_(sock); close(sock); putlog("daemonize(): UNREACHABLE CODE REACHED!"); signals(0); } /**************** CLIENT FUNCTIONS ****************/ /** * @brief sock_send_data - send data to a socket */ void sock_send_data(const char *host, const char *port, const char *data){ int sock = 0; struct addrinfo hints, *res, *p; memset(&hints, 0, sizeof(hints)); hints.ai_family = AF_INET; hints.ai_socktype = SOCK_STREAM; hints.ai_flags = AI_PASSIVE; if(getaddrinfo(host, port, &hints, &res) != 0){ ERR("getaddrinfo failed"); } struct sockaddr_in *ia = (struct sockaddr_in*)res->ai_addr; char str[INET_ADDRSTRLEN]; inet_ntop(AF_INET, &(ia->sin_addr), str, INET_ADDRSTRLEN); DBG("canonname: %s, port: %u, addr: %s\n", res->ai_canonname, ntohs(ia->sin_port), str); // loop through all the results and bind to the first we can for(p = res; p != NULL; p = p->ai_next){ if((sock = socket(p->ai_family, p->ai_socktype, p->ai_protocol)) == -1){ continue; } if(connect(sock, p->ai_addr, p->ai_addrlen) == -1){ close(sock); continue; } break; // if we get here, we have a successfull connection } if(p == NULL) ERRX("failed to connect to server"); size_t L = strlen(data); if(send(sock, data, L, 0) != (ssize_t)L){ WARN("send"); return;} double t0 = dtime(); while(dtime() - t0 < SOCKET_TIMEOUT){ if(!waittoread(sock)) continue; char buff[32]; int n = read(sock, buff, 31); if(n > 0){ buff[n] = 0; printf("%s\n", buff); close(sock); return; } } WARN("no answer!"); close(sock); }