modbus-slave/src/stcpsc.c

/**
 * \file stcpsc.c
 * \brief Simple-to-use TCP server/client
 * \author Petr Svoboda, 2014-10-11
 *
 * 2024 - removed protocol handling, just using it to send and receive raw frames. Added static.
 */

#include "stcpsc.h"
#include <arpa/inet.h>
#include <errno.h>
#include <netdb.h>
#include <netinet/in.h>
#include <pthread.h>
#include <semaphore.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <unistd.h>

#define STCP_MAX_MSG_SIZE 2048
#define STCP_HOST_ADR_LEN 100

#define STCP_READ_ERR_NO 0
#define STCP_READ_ERR_CLOSE STCP_DISCONNECT_REASON_CLOSED
#define STCP_READ_ERR_READ STCP_DISCONNECT_REASON_ERROR
#define STCP_READ_ERR_INTERNAL STCP_DISCONNECT_REASON_INTERNAL
#define STCP_READ_ERR_MTU STCP_DISCONNECT_REASON_MTU
#define STCP_READ_ERR_KICK STCP_DISCONNECT_REASON_KICK

struct simple_tcp_queue;
typedef struct simple_tcp_queue simple_tcp_queue_t;

struct simple_tcp_queue {
    int cli_fd, mtu;
    msg_received_fce_t cli_rec_fce;
    simple_tcp_queue_t *next;
};

/* Server stuff */
static volatile int srv_done = 0;
static unsigned short srv_port = 1234;
static int srv_mtu = -1;
static simple_tcp_queue_t *srv_queue = NULL;
static pthread_t server_thread = 0;
static fd_set active_fd_set;

static msg_received_fce_t srv_rec_fce = NULL;
static cli_connected_fce_t srv_conn_fce = NULL;
static cli_disconnected_fce_t srv_disconn_fce = NULL;

/* Client stuff */
static sem_t cli_conn_sem;
static int tcpcli_portno;
static int client_mtu = -1;
static volatile int tcpcli_sockfd = -1;
static volatile int cli_thread_done = 0;
static pthread_t client_thread;
static char cli_host_address[STCP_HOST_ADR_LEN + 1];

static msg_received_fce_t cli_rec_fce = NULL;


bool simple_tcp_client_ended() { return cli_thread_done; }

bool simple_tcp_server_ended() { return srv_done; }

static int make_server_socket() {
    int server_sock;
    struct sockaddr_in name;
    int yes = 1;

    /* Create the socket. */
    server_sock = socket(PF_INET, SOCK_STREAM, 0);
    if (server_sock < 0) {
        printf("TCPSRV: socket err\n");
        return -1;
    }

    // tento radek zpusobi, ze pri opakovanem restartu serveru, bude volani
    // funkce bind() uspesne, kdo neveri, at ho zakomentuje :))
    if (setsockopt(server_sock, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(yes)) == -1) {
        printf("TCPSRV: setsockopt err\n");
        return -1;
    }

    /* Give the socket a name. */
    name.sin_family = AF_INET;
    name.sin_port = htons(srv_port);
    name.sin_addr.s_addr = htonl(INADDR_ANY);
    if (bind(server_sock, (struct sockaddr *) &name, sizeof(name)) < 0) {
        printf("TCPSRV: bind err\n");
        return -1;
    }

    return server_sock;
}

static void close_client(int fd, int reason) {
    simple_tcp_queue_t *toDel = srv_queue;
    simple_tcp_queue_t **prev = &srv_queue;
    while (toDel) {
        if (toDel->cli_fd == fd) {
            /* match! */
            simple_tcp_queue_t *next = toDel->next;
            free(toDel);
            *prev = next;
            break;
        }
        /* iterate */
        prev = &(toDel->next);
        toDel = toDel->next;
    }
    FD_CLR(fd, &active_fd_set); /* Remove the client from the set */
    close(fd);
    if (srv_disconn_fce)
        srv_disconn_fce(fd, reason);
}

static int read_from_client(simple_tcp_queue_t *cli) {
    if (cli == NULL) {
        return STCP_READ_ERR_INTERNAL;
    }

    // limitation - it must all come in one frame!
    unsigned char buff[cli->mtu];
    int readed = recv(cli->cli_fd, buff, STCP_MAX_MSG_SIZE + 2, 0);

    simple_msg_t msg = {
            .data = buff,
            .len = readed,
    };

    if (cli->cli_rec_fce(cli->cli_fd, &msg)) {
        return STCP_READ_ERR_KICK;
    }

    return STCP_READ_ERR_NO;
}

static int read_from_srv_fd(int filedes) {
    simple_tcp_queue_t *cli = srv_queue;
    while (cli) {
        if (cli->cli_fd == filedes) {
            break;
        }
        cli = cli->next;
    }
    return read_from_client(cli);
}

static void *srv_thread_function(void *arg) {
    struct timeval tim;
    int server_sock;
    fd_set read_fd_set;
    int fd, s_rv;
    struct sockaddr_in clientname;
    socklen_t size;
    printf("srv_thread_function\n");

    /* unused */
    (void) arg;

    /* Create the socket and set it up to accept connections. */
    server_sock = make_server_socket();
    if (server_sock < 0) {
        printf("TCPSRV: fail to create server socket\n");
        return NULL;
    }
    printf("Socket bound\n");

    if (listen(server_sock, 1) < 0) {
        printf("TCPSRV: listen ERROR\n");
        return NULL;
    }
    printf("Server awaiting clients...\n");

    /* Initialize the set of active sockets. */
    FD_ZERO(&active_fd_set);
    FD_SET(server_sock, &active_fd_set);

    while (!srv_done) {
        /* 200ms timeout */
        tim.tv_sec = 0;
        tim.tv_usec = 200000;
        /* Block until input arrives on one or more active sockets. */
        read_fd_set = active_fd_set;
        s_rv = select(FD_SETSIZE, &read_fd_set, NULL, NULL, &tim);

        if (s_rv == 0) {
            continue;
        }

        if (s_rv < 0) {
            printf("TCPSRV: server select ERROR\n");
            return NULL;
        }

        printf("TCPSRV: client selected\n");
        /* Service all the sockets with input pending. */
        for (fd = 0; fd < FD_SETSIZE; ++fd) {
            if (FD_ISSET(fd, &read_fd_set)) {
                if (fd == server_sock) {
                    /* Connection request on original socket. */
                    int novy;
                    size = sizeof(clientname);
                    novy = accept(server_sock, (struct sockaddr *) &clientname, &size);
                    if (novy < 0) {
                        printf("TCPSRV: server accept ERROR\n");
                        return NULL;
                    }
                    printf("TCPSRV: client accepted\n");

                    FD_SET(novy, &active_fd_set); /* Add new client to the set */
                    simple_tcp_queue_t *cliq = (simple_tcp_queue_t *) malloc(sizeof(simple_tcp_queue_t));
                    cliq->cli_fd = novy;
                    cliq->next = srv_queue;
                    cliq->mtu = srv_mtu;
                    cliq->cli_rec_fce = srv_rec_fce;
                    srv_queue = cliq;

                    if (srv_conn_fce) {
                        if (srv_conn_fce(novy, inet_ntoa(clientname.sin_addr))) {
                            close_client(novy, STCP_DISCONNECT_REASON_KICK);
                        }
                    }
                } else {
                    /* Data arriving on an already-connected socket. */
                    printf("TCPSRV: receiving data from client\n");
                    int crv = read_from_srv_fd(fd);
                    if (crv != STCP_READ_ERR_NO) {
                        close_client(fd, crv);
                    }
                }
            }
        }
    }

    /* Close all open sockets */
    for (fd = 0; fd < FD_SETSIZE; ++fd) {
        if (FD_ISSET(fd, &read_fd_set)) {
            close_client(fd, STCP_DISCONNECT_REASON_SERVER_CLOSE);
        }
    }
    return NULL;
}

static void *cli_thread_function(void *arg) {
    struct timeval tim;
    struct sockaddr_in host_addr;
    struct hostent *host_ent;
    fd_set active_fd_set;
    fd_set read_fd_set;
    int fd, s_rv;
    char host_ip_str[INET_ADDRSTRLEN];

    /* unused */
    (void) arg;

    tcpcli_sockfd = -1;

    /* Create a socket point */
    tcpcli_sockfd = socket(AF_INET, SOCK_STREAM, 0);
    if (tcpcli_sockfd < 0) {
        printf("TCPCL: error opening socket\n");
        sem_post(&cli_conn_sem);
        return NULL;
    }

    host_ent = gethostbyname(cli_host_address);
    if (host_ent == NULL) {
        printf("TCPCL: error '%s': no such host\n", cli_host_address);
        tcpcli_sockfd = -1;
        sem_post(&cli_conn_sem);
        return NULL;
    }

    memset(&host_addr, 0, sizeof(host_addr));
    host_addr.sin_family = AF_INET;
    host_addr.sin_port = htons(tcpcli_portno);
    memcpy(&host_addr.sin_addr.s_addr, host_ent->h_addr, host_ent->h_length);
    inet_ntop(AF_INET, &(host_addr.sin_addr), host_ip_str, INET_ADDRSTRLEN);

    printf("TCPCL: connecting to %s:%d\n", host_ip_str, tcpcli_portno);
    /* Now connect to the server */
    if (connect(tcpcli_sockfd, (struct sockaddr *) &host_addr, sizeof(host_addr)) < 0) {
        printf("TCPCL: error connect to: %s:%d\n", host_ip_str, tcpcli_portno);
        printf("error: %s\n", strerror(errno));
        tcpcli_sockfd = -1;
        sem_post(&cli_conn_sem);
        return NULL;
    }
    simple_tcp_queue_t *client_que;

    printf("TCPCL: connected to %s:%d\n", host_ip_str, tcpcli_portno);
    sem_post(&cli_conn_sem);

    client_que = (simple_tcp_queue_t *) malloc(sizeof(simple_tcp_queue_t));
    client_que->cli_fd = tcpcli_sockfd;
    client_que->next = NULL;
    client_que->mtu = client_mtu;
    client_que->cli_rec_fce = cli_rec_fce;

    /* Initialize the set of active sockets. */
    FD_ZERO(&active_fd_set);
    FD_SET(tcpcli_sockfd, &active_fd_set);

    while (!cli_thread_done) {
        /* 200ms timeout */
        tim.tv_sec = 0;
        tim.tv_usec = 200000;
        /* Block until input arrives on one or more active sockets. */
        read_fd_set = active_fd_set;
        s_rv = select(FD_SETSIZE, &read_fd_set, NULL, NULL, &tim);

        if (s_rv == 0) {
            continue;
        }

        if (s_rv < 0) {
            printf("TCPSRV: server select ERROR\n");
            return NULL;
        }

        /* Service all the sockets with input pending. */
        for (fd = 0; fd < FD_SETSIZE; ++fd) {
            if (FD_ISSET(fd, &read_fd_set) && fd == tcpcli_sockfd) {
                if (read_from_client(client_que) != STCP_READ_ERR_NO) {
                    cli_thread_done = 1;
                }
            }
        }
    }

    free(client_que);
    close(tcpcli_sockfd);
    tcpcli_sockfd = -1;
    return NULL;
}

int simple_tcp_server_start(unsigned short port, int mtu, msg_received_fce_t rec, cli_connected_fce_t con,
                            cli_disconnected_fce_t dis) {
    srv_done = 0;
    srv_port = port;
    srv_mtu = mtu;
    srv_rec_fce = rec;
    srv_conn_fce = con;
    srv_disconn_fce = dis;
    srv_queue = NULL;
    if (pthread_create(&server_thread, NULL, srv_thread_function, NULL)) {
        printf("Fail to create server thread\n");
        srv_done = 1;
        return -1;
    }
    return 0;
}

int simple_tcp_server_stop() {
    if (srv_done || !server_thread)
        return 0;
    srv_done = 1;
    if (pthread_join(server_thread, NULL))
        return -1;
    return 0;
}

int simple_tcp_client_start(const char *address_port, int mtu, int def_port, msg_received_fce_t rec) {
    char *addrtempbuf = strdup(address_port);

    char *portp = strchr(addrtempbuf, ':');
    if (portp) {
        *portp = '\0';
        portp++;
        tcpcli_portno = atoi((portp));
    } else {
        tcpcli_portno = def_port;
    }

    cli_thread_done = 0;
    client_mtu = mtu;
    cli_rec_fce = rec;
    strncpy(cli_host_address, addrtempbuf, STCP_HOST_ADR_LEN);

    free(addrtempbuf);

    if (sem_init(&cli_conn_sem, 0, 0) < 0) {
        printf("TCPCL: sem_init\n");
        return -1;
    }

    if (pthread_create(&client_thread, NULL, cli_thread_function, NULL)) {
        cli_thread_done = 1;
        printf("Failed to create pthread");
        return -1;
    }

    sem_wait(&cli_conn_sem);
    sem_destroy(&cli_conn_sem);

    return tcpcli_sockfd;
}

int simple_tcp_client_done() {
    if (cli_thread_done) {
        return 0;
    }
    cli_thread_done = 1;
    if (pthread_join(client_thread, NULL)) {
        return -1;
    }
    return 0;
}

int simple_tcp_send(int fd, const simple_msg_t *msg) {
    write(fd, msg->data, msg->len);
    return 0;
}