mirror of
https://github.com/eddyem/eddys_snippets.git
synced 2026-03-20 08:41:02 +03:00
add serialsock
This commit is contained in:
386
serialsock/sersock.c
Normal file
386
serialsock/sersock.c
Normal file
@@ -0,0 +1,386 @@
|
||||
/*
|
||||
* This file is part of the canserver project.
|
||||
* Copyright 2022 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 <arpa/inet.h>
|
||||
#include <limits.h>
|
||||
#include <netdb.h>
|
||||
#include <poll.h>
|
||||
#include <signal.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/select.h>
|
||||
#include <sys/socket.h>
|
||||
#include <sys/types.h>
|
||||
#include <sys/un.h> // unix socket
|
||||
#include <usefull_macros.h>
|
||||
|
||||
#include "cmdlnopts.h"
|
||||
#include "sersock.h"
|
||||
|
||||
/**
|
||||
* 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 = 0;
|
||||
timeout.tv_usec = 100;
|
||||
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)){
|
||||
//DBG("FD_ISSET");
|
||||
return 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
// work with single client, return FALSE if disconnected
|
||||
static int handle_socket(int sock, TTY_descr *d){
|
||||
char buff[BUFLEN];
|
||||
ssize_t rd = read(sock, buff, BUFLEN-1);;
|
||||
DBG("Got %zd bytes", rd);
|
||||
if(rd <= 0){ // error or disconnect
|
||||
DBG("Nothing to read from fd %d (ret: %zd)", sock, rd);
|
||||
return FALSE;
|
||||
}
|
||||
// add trailing zero to be on the safe side
|
||||
buff[rd] = 0;
|
||||
DBG("GOT: %s", buff);
|
||||
ssize_t blen = strlen(buff);
|
||||
if(blen != write(d->comfd, buff, blen)){
|
||||
LOGWARN("write()");
|
||||
WARN("write()");
|
||||
}
|
||||
if(buff[blen-1] == '\n') buff[blen-1] = 0;
|
||||
LOGMSG("CLIENT_%d: %s", sock, buff);
|
||||
return TRUE;
|
||||
}
|
||||
|
||||
/**
|
||||
* check data from fd
|
||||
* @param fd - file descriptor
|
||||
* @return 0 in case of timeout, 1 in case of fd have data, -1 if error
|
||||
*/
|
||||
static int canberead(int fd){
|
||||
fd_set fds;
|
||||
struct timeval timeout;
|
||||
timeout.tv_sec = 0;
|
||||
timeout.tv_usec = 100;
|
||||
FD_ZERO(&fds);
|
||||
FD_SET(fd, &fds);
|
||||
do{
|
||||
int rc = select(fd+1, &fds, NULL, NULL, &timeout);
|
||||
if(rc < 0){
|
||||
if(errno != EINTR){
|
||||
LOGWARN("select()");
|
||||
WARN("select()");
|
||||
return -1;
|
||||
}
|
||||
continue;
|
||||
}
|
||||
break;
|
||||
}while(1);
|
||||
if(FD_ISSET(fd, &fds)){
|
||||
DBG("FD_ISSET");
|
||||
return 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief getserdata - read data (ending by '\n') from serial device
|
||||
* @param D (i) - device
|
||||
* @param len (o) - amount of butes read (-1 if disconnected)
|
||||
* @return pointer to data buffer or NULL if none
|
||||
*/
|
||||
static char *getserdata(TTY_descr *D, int *len){
|
||||
static char serbuf[BUFLEN], *ptr = serbuf;
|
||||
static size_t blen = BUFLEN - 1;
|
||||
if(!D || D->comfd < 0) return NULL;
|
||||
char *nl = NULL;
|
||||
do{
|
||||
int s = canberead(D->comfd);
|
||||
if(s == 0) break;
|
||||
if(s < 0){ // interrupted?
|
||||
if(len) *len = 0;
|
||||
return NULL;
|
||||
}
|
||||
ssize_t l = read(D->comfd, ptr, blen);
|
||||
if(l < 1){ // disconnected
|
||||
DBG("device disconnected");
|
||||
if(len) *len = -1;
|
||||
return NULL;
|
||||
}
|
||||
ptr[l] = 0;
|
||||
DBG("GOT %zd bytes: '%s'", l, ptr);
|
||||
nl = strchr(ptr, '\n');
|
||||
ptr += l;
|
||||
blen -= l;
|
||||
if(nl){
|
||||
DBG("Got newline");
|
||||
break;
|
||||
}
|
||||
}while(blen);
|
||||
// recalculate newline from the beginning (what if old data stays there?)
|
||||
nl = strchr(serbuf, '\n');
|
||||
if(blen && !nl){
|
||||
if(len) *len = 0;
|
||||
return NULL;
|
||||
}
|
||||
// in case of overflow send buffer without trailing '\n'
|
||||
int L;
|
||||
if(nl) L = nl - serbuf + 1; // get line to '\n'
|
||||
else L = strlen(serbuf); // get all buffer
|
||||
if(len) *len = L;
|
||||
memcpy(D->buf, serbuf, L); // copy all + trailing zero
|
||||
D->buflen = L;
|
||||
D->buf[L] = 0;
|
||||
DBG("Put to buf %d bytes: '%s'", L, D->buf);
|
||||
if(nl){
|
||||
L = ptr - nl - 1; // symbols after newline
|
||||
if(L > 0){ // there's some data after '\n' -> put it into the beginning
|
||||
memmove(serbuf, nl+1, L);
|
||||
blen = BUFLEN - 1 - L;
|
||||
ptr = serbuf + L;
|
||||
*ptr = 0;
|
||||
DBG("now serbuf is '%s'", serbuf);
|
||||
}else{
|
||||
blen = BUFLEN - 1;
|
||||
ptr = serbuf;
|
||||
*ptr = 0;
|
||||
}
|
||||
}else{
|
||||
blen = BUFLEN - 1;
|
||||
ptr = serbuf;
|
||||
*ptr = 0;
|
||||
}
|
||||
return D->buf;
|
||||
}
|
||||
|
||||
static void server_(int sock, TTY_descr *d){
|
||||
if(listen(sock, MAXCLIENTS) == -1){
|
||||
WARN("listen");
|
||||
LOGWARN("listen");
|
||||
return;
|
||||
}
|
||||
int enable = 1;
|
||||
if(ioctl(sock, FIONBIO, (void *)&enable) < 0){ // make socket nonblocking
|
||||
LOGERR("Can't make socket nonblocking");
|
||||
ERRX("ioctl()");
|
||||
}
|
||||
int nfd = 1; // only one socket @start
|
||||
struct pollfd poll_set[MAXCLIENTS+1];
|
||||
bzero(poll_set, sizeof(poll_set));
|
||||
// ZERO - listening server socket
|
||||
poll_set[0].fd = sock;
|
||||
poll_set[0].events = POLLIN;
|
||||
while(1){
|
||||
poll(poll_set, nfd, 1); // max timeout - 1ms
|
||||
if(poll_set[0].revents & POLLIN){ // check main for accept()
|
||||
struct sockaddr_in addr;
|
||||
socklen_t len = sizeof(addr);
|
||||
int client = accept(sock, (struct sockaddr*)&addr, &len);
|
||||
DBG("New connection");
|
||||
LOGMSG("Connection, fd=%d", client);
|
||||
if(nfd == MAXCLIENTS + 1){
|
||||
LOGWARN("Max amount of connections, disconnect fd=%d", client);
|
||||
WARNX("Limit of connections reached");
|
||||
close(client);
|
||||
}else{
|
||||
memset(&poll_set[nfd], 0, sizeof(struct pollfd));
|
||||
poll_set[nfd].fd = client;
|
||||
poll_set[nfd].events = POLLIN;
|
||||
++nfd;
|
||||
}
|
||||
}
|
||||
int l = -1;
|
||||
char *serdata = getserdata(d, &l);
|
||||
if(l < 0){
|
||||
LOGERR("Serial device disconnected");
|
||||
ERRX("Serial device disconnected");
|
||||
}
|
||||
if(serdata){
|
||||
for(int i = 1; i < nfd; ++i)
|
||||
if(l != write(poll_set[i].fd, serdata, l)){
|
||||
LOGWARN("write()");
|
||||
WARN("write()");
|
||||
}
|
||||
if(serdata[l-1] == '\n') serdata[l-1] = 0;
|
||||
LOGMSG("SERIAL: %s", serdata);
|
||||
}
|
||||
// scan connections
|
||||
for(int fdidx = 1; fdidx < nfd; ++fdidx){
|
||||
if((poll_set[fdidx].revents & POLLIN) == 0) continue;
|
||||
int fd = poll_set[fdidx].fd;
|
||||
if(!handle_socket(fd, d)){ // socket closed
|
||||
DBG("Client fd=%d disconnected", fd);
|
||||
LOGMSG("Client fd=%d disconnected", fd);
|
||||
close(fd);
|
||||
// move last FD to current position
|
||||
poll_set[fdidx] = poll_set[nfd - 1];
|
||||
--nfd;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// read console char - for client
|
||||
static int rc(){
|
||||
int rb;
|
||||
struct timeval tv;
|
||||
int retval;
|
||||
fd_set rfds;
|
||||
FD_ZERO(&rfds);
|
||||
FD_SET(STDIN_FILENO, &rfds);
|
||||
tv.tv_sec = 0; tv.tv_usec = 100;
|
||||
retval = select(1, &rfds, NULL, NULL, &tv);
|
||||
if(!retval) rb = 0;
|
||||
else {
|
||||
if(FD_ISSET(STDIN_FILENO, &rfds)) rb = getchar();
|
||||
else rb = 0;
|
||||
}
|
||||
return rb;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief mygetline - silently and non-blocking getline
|
||||
* @return zero-terminated string with '\n' at end (or without in case of overflow)
|
||||
*/
|
||||
static char *mygetline(){
|
||||
static char buf[BUFLEN+1];
|
||||
static int i = 0;
|
||||
while(i < BUFLEN){
|
||||
char rd = rc();
|
||||
if(!rd) return NULL;
|
||||
if(rd == 0x7f && i){ // backspace
|
||||
buf[--i] = 0;
|
||||
printf("\b \b");
|
||||
}else{
|
||||
buf[i++] = rd;
|
||||
printf("%c", rd);
|
||||
}
|
||||
fflush(stdout);
|
||||
if(rd == '\n') break;
|
||||
}
|
||||
buf[i] = 0;
|
||||
i = 0;
|
||||
return buf;
|
||||
}
|
||||
|
||||
static void client_(int sock){
|
||||
setup_con(); // convert console mode into non-canon
|
||||
int Bufsiz = BUFLEN;
|
||||
char *recvBuff = MALLOC(char, Bufsiz);
|
||||
while(1){
|
||||
char *msg = mygetline();
|
||||
if(msg){
|
||||
ssize_t L = strlen(msg);
|
||||
if(send(sock, msg, L, 0) != L){
|
||||
WARN("send");
|
||||
WARN("send");
|
||||
}
|
||||
if(msg[L-1] == '\n') msg[L-1] = 0;
|
||||
LOGMSG("TERMINAL: %s", msg);
|
||||
}
|
||||
if(!waittoread(sock)) continue;
|
||||
int n = read(sock, recvBuff, Bufsiz-1);
|
||||
if(n == 0){
|
||||
WARNX("Server disconnected");
|
||||
signals(0);
|
||||
}
|
||||
recvBuff[n] = 0;
|
||||
printf("%s", recvBuff);
|
||||
if(recvBuff[n-1] == '\n') recvBuff[n-1] = 0;
|
||||
LOGMSG("SERIAL: %s", recvBuff);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief openserialdev - open connection to serial device
|
||||
* @param path (i) - path to device
|
||||
* @param speed - connection speed
|
||||
* @return pointer to device structure if all OK
|
||||
*/
|
||||
static TTY_descr *openserialdev(char *path, int speed){
|
||||
TTY_descr *d = new_tty(path, speed, BUFLEN);
|
||||
DBG("Device created");
|
||||
if(!d || !(tty_open(d, 1))){
|
||||
WARN("Can't open device %s", path);
|
||||
LOGWARN("Can't open device %s", path);
|
||||
return NULL;
|
||||
}
|
||||
DBG("device opened");
|
||||
return d;
|
||||
}
|
||||
|
||||
int start_socket(int server, char *path, TTY_descr **dev){
|
||||
if(server){
|
||||
if(!dev) return 1;
|
||||
if(!(*dev = openserialdev(GP->devpath, GP->speed))){
|
||||
LOGERR("Can't open serial device %s", GP->devpath);
|
||||
ERR("Can't open serial device %s", GP->devpath);
|
||||
}
|
||||
}
|
||||
int sock = -1;
|
||||
int reuseaddr = 1;
|
||||
struct sockaddr_un saddr = {0};
|
||||
saddr.sun_family = AF_UNIX;
|
||||
strncpy(saddr.sun_path, path, 106); // if sun_path[0] == 0 we don't create a file
|
||||
if((sock = socket(AF_UNIX, SOCK_SEQPACKET, 0)) < 0){ // or SOCK_STREAM?
|
||||
LOGERR("socket()");
|
||||
ERR("socket()");
|
||||
}
|
||||
if(server){
|
||||
if(setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &reuseaddr, sizeof(int)) == -1){
|
||||
LOGWARN("setsockopt");
|
||||
WARN("setsockopt");
|
||||
}
|
||||
if(bind(sock, &saddr, sizeof(saddr)) == -1){
|
||||
close(sock);
|
||||
LOGERR("bind");
|
||||
ERR("bind");
|
||||
}
|
||||
}else{
|
||||
if(connect(sock, &saddr, sizeof(saddr)) == -1){
|
||||
LOGERR("connect()");
|
||||
ERR("connect()");
|
||||
}
|
||||
}
|
||||
if(server) server_(sock, *dev);
|
||||
else client_(sock);
|
||||
close(sock);
|
||||
signals(0);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user