serialmux v1.0

Signed-off-by: leyyer <15135994+leyyer@user.noreply.gitee.com>

Makefile

@@ -2,11 +2,13 @@ CROSS_COMPILE ?= aarch64-none-linux-gnu-
 
 CC := $(CROSS_COMPILE)gcc
 LD := $(CROSS_COMPILE)gcc
+VER :=1.0
+PTMPDIR :=seialmux-$(VER)
 
-CFLAGS += -D_GNU_SOURCE -std=gnu99 -Wall
+CFLAGS := -D_GNU_SOURCE -std=gnu99 -Wall -fno-strict-aliasing -O2
 CFLAGS += -Ilibev -DNDEBUG
 
-LDFLAGS += -static -lutil -lm -Llibs -lev
+LDFLAGS += -static -lutil -lm -Llibs -lev -Wl,-s
 
 OBJS := main.o dictionary.o iniparser.o
 
@@ -15,3 +17,13 @@ serialmux: $(OBJS)
 
 clean:
 	@rm -rf $(OBJS)
+
+pkg: serialmux serialmux.ini serialmux.service install.sh
+	@rm -f serialmux-v$(VER).tar
+	@rm -f serialmux-v$(VER).tar.xz
+	@mkdir -p $(PTMPDIR)
+	@cp $^ $(PTMPDIR)
+	@tar cf serialmux-v$(VER).tar $(PTMPDIR)
+	@xz -9 serialmux-v$(VER).tar
+	@rm -rf $(PTMPDIR)
+

install.sh

@@ -0,0 +1,12 @@
+#/bin/bash
+
+if [ `id -u` != 0 ]; then
+	echo "Plese use root."
+	exit -1
+fi
+
+install -m 0644 serialmux.service /usr/lib/systemd/system
+install -m 0644 serialmux.ini /etc
+install -m 0755 serialmux /usr/local/bin
+
+systemctl enable serialmux

main.c

@@ -1,3 +1,7 @@
+/*
+ * socat -d -d PTY,raw,echo=0,link=/dev/ttyVA0 PTY,raw,echo=0,link=/dev/ttyVB0
+ * socat -d -d open:/dev/ttyVA0,nonblock,echo=0,raw TCP-LISTEN:6000,reuseaddr,fork
+ */
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -7,29 +11,155 @@
 #include <sys/poll.h>
 #include <fcntl.h>
 #include <errno.h>
-#include <grp.h>
+#include <signal.h>
 #include <pty.h>
+#include <ev.h>
+#include "iniparser.h"
+#include "libev/ev.h"
+
+#define CONFIG_FILE "/etc/serialmux.ini"
+#define BUFLEN  (128)
+#define MIN_LEN (6)
 
 struct tty_mux_session {
+	ev_io base;
+
 	struct tty_mux_session *next;
 	char name[64];
+
+	int connected;
+
 	int mfd;
 	int sfd;
+
 	int index;
 
-	uint8_t in[1024];
+	uint8_t in[BUFLEN];
 	int insize;
-
-	uint8_t out[1024];
-	int outsize;
 };
 
-static struct tty_mux_session *sesions = NULL;
+static struct tty_mux_session *sessions = NULL;
+static int debug_verbose = 0;
+static int uart_fd = -1;
+
+static int write_all(int fd, char *ptr, int len)
+{
+	int tot = 0;
+	int nr;
+
+	while (len > 0) {
+		nr = write(fd, ptr, len);
+		if (nr < 0 && (errno == EINTR || errno == EAGAIN)) {
+			continue;
+		}
+
+		if (nr < 0) {
+			break;
+		}
+		if (nr > 0) {
+			ptr += nr;
+			len -= nr;
+			tot += nr;
+		}
+	}
+
+	return (tot);
+}
+
+static void forward_to_uart(struct tty_mux_session *tms)
+{
+	int begin = 0;
+	int wroten;
+
+	/* find the packet */
+reparse:
+	if (tms->insize < MIN_LEN) {
+		goto end;
+	}
+
+	while ((begin < tms->insize) && (tms->in[begin] != 0x23))
+		++begin;
+
+	if (begin >= tms->insize) {
+		goto end;
+	}
+
+	if ((begin < tms->insize) && (tms->in[begin + 5] != 0x0A)) {
+		++begin;
+		goto reparse;
+	}
+
+	if ((tms->insize - begin) < MIN_LEN) {
+		goto end;
+	}
+
+	wroten = write_all(uart_fd, (char *)tms->in + begin, MIN_LEN);
+	if (wroten != MIN_LEN) {
+		printf("pty(%s) send incomplete (%d of %d)\n", tms->name, wroten, MIN_LEN);
+	}
+
+	if (debug_verbose) {
+		printf("wroten %d bytes to uart\n", wroten);
+	}
+
+	begin += MIN_LEN;
+
+end:
+	if (begin > 0) {
+		tms->insize -= begin;
+		if (tms->insize > 0) {
+			memmove(tms->in, tms->in + begin, tms->insize);
+			if (debug_verbose) {
+				printf("pty %s has %d bytes\n", tms->name, tms->insize);
+			}
+		}
+	}
+}
+
+static void __reading_pty(EV_P_ ev_io *base, int events)
+{
+	struct tty_mux_session *tms = (struct tty_mux_session *)(base);
+
+	if (!tms->connected) {
+		if (debug_verbose) {
+			printf("%s opened\n", tms->name);
+		}
+		tms->connected = 1;
+	}
+
+	if (tms->insize >= MIN_LEN) {
+		forward_to_uart(tms);
+	}
+
+	if (events & EV_READ) {
+		int nr = read(base->fd, tms->in + tms->insize, BUFLEN - tms->insize);
+		if (nr > 0) {
+			if (debug_verbose) {
+				int i;
+				printf("%s read (%d):\n", tms->name, nr);
+				for (i = 0; i < nr; ++i) {
+					printf("%02x%c", tms->in[tms->insize + i], (i + 1) % 16 ? ' ' : '\n');
+				}
+				printf("\n");
+			}
+			tms->insize += nr;
+		}
+		if (nr == 0) {
+			printf("read 0 bytes\n");
+		}
+	}
+
+	if (tms->insize >= MIN_LEN) {
+		forward_to_uart(tms);
+	}
+}
 
 struct tty_mux_session *session_alloc(int index, const char *name_prefix)
 {
 	char name[64];
 	struct tty_mux_session *mux;
+	int flags;
+	struct termios opts;
 
 	mux = calloc(1, sizeof *mux);
 	if (mux == NULL) {
@@ -43,13 +173,25 @@ struct tty_mux_session *session_alloc(int index, const char *name_prefix)
 		return NULL;
 	}
 
+	flags = fcntl(mux->mfd, F_GETFL);
+	flags |= O_NONBLOCK;
+	fcntl(mux->mfd, F_SETFL, flags);
+
+	if (tcgetattr(mux->mfd, &opts) == 0) {
+		cfmakeraw(&opts);
+		tcsetattr(mux->mfd,TCSANOW,&opts);
+	}
+
 	snprintf(mux->name, sizeof mux->name, "%s%d", name_prefix, index);
 
 	unlink(mux->name);
 	symlink(name, mux->name);
 
+	mux->connected = 0;
 	mux->index = index;
 
+	ev_io_init(&mux->base, __reading_pty, mux->mfd, EV_READ);
+
 	return mux;
 }
 
@@ -154,36 +296,127 @@ int serial_setopt(int fd,int nSpeed, int nBits, char nEvent, int nStop)
 	return 0;
 }
 
-int main(int argc, char *argv[])
+static void forward_to_ptys(char *ptr, int len)
 {
-	int sfd = -1, mfd = -1;
-	char name[32];
-	char buf[1024];
-	int n;
-	struct group *grp;
+	struct tty_mux_session *tms;
+	int wroten;
+
+	tms = sessions;
+	while (tms) {
+		/* if (tms->connected) { */
+			wroten = write_all(tms->mfd, ptr, len);
+			if (debug_verbose) {
+				printf("write to %s %d bytes, expect %d bytes\n", tms->name, wroten, len);
+			}
+		/* } */
+		tms = tms->next;
+	}
+}
 
-	grp = getgrnam("dialout");
+static void reading_uart(EV_P_ ev_io *base, int events)
+{
+	char buf[BUFLEN];
+	int nr;
 
-	if (grp) {
-		printf("tty: %d\n", grp->gr_gid);
-		setgid(grp->gr_gid);
+	if (events & EV_READ) {
+		nr = read(uart_fd, buf, BUFLEN);
+		if (nr > 0) {
+			if (debug_verbose) {
+				int i;
+				printf("uart reading (%d):\n", nr);
+				for (i = 0; i < nr; ++i) {
+					printf("%02x%c", buf[i], (i + 1) % 16 ? ' ' : '\n');
+				}
+				printf("\n");
+			}
+			forward_to_ptys(buf, nr);
+		}
 	}
+}
+
+int main(int argc, char *argv[])
+{
+	int i;
+	FILE *sfp;
+	dictionary *ini;
+	const char *serdev;
+	const char *name_prefix;
+	int muxdev;
+	int brd, databits, stopbits, parity;
+	struct ev_loop *loop;
+	ev_io uart_io;
+
+	signal(SIGPIPE, SIG_IGN);
 
-	if (openpty(&mfd, &sfd, name, NULL, NULL) == -1) {
-		printf("can't openpty\r\n");
+	sfp = fopen(CONFIG_FILE, "rb");
+	if (sfp == NULL) {
+		fprintf(stderr, "Can't open %s to read\n", CONFIG_FILE);
 		return -1;
 	}
 
+	ini = iniparser_load_file(sfp, CONFIG_FILE);
+	fclose(sfp);
+
+	if (!ini) {
+		fprintf(stderr, "%s is not a valid ini file\n", CONFIG_FILE);
+		return -2;
+	}
+
+	serdev = iniparser_getstring(ini, "serial:device", "/dev/ttyS1");
+
+	brd = iniparser_getint(ini, "serial:baudrate", 115200);
+	databits = iniparser_getint(ini, "serial:databits", 8);
+	stopbits = iniparser_getint(ini, "serial:stopbits", 1);
+	parity = iniparser_getstring(ini, "serial:parity", "N")[0];
+
+	muxdev = iniparser_getint(ini, "mux:num", 1);
+	name_prefix=iniparser_getstring(ini, "mux:device", "/dev/ttyMUX");
+	debug_verbose = iniparser_getint(ini, "debug:verbose", 0);
+
+	loop = EV_DEFAULT;
+
+	uart_fd = open(serdev, O_RDWR | O_NONBLOCK);
+	if (uart_fd < 0) {
+		fprintf(stderr, "[%d] Can't open %s\n", __LINE__, serdev);
+		return -3;
+	}
 
-	unlink("/dev/ttySX0");
-	symlink(name, "/dev/ttySX0");
-	printf("sfd=%d, mfd=%d, name=%s\r\n", sfd, mfd, name);
+	serial_setopt(uart_fd, brd, databits, parity, stopbits);
 
-	while (1) {
-		n = read(mfd, buf, sizeof buf);
-		if (n > 0) {
-			write(mfd, buf, n);
+	ev_io_init(&uart_io, reading_uart, uart_fd, EV_READ);
+	ev_io_start(loop, &uart_io);
+
+	if (debug_verbose) {
+		printf("%s %d,%d%c%d\n", serdev, brd, databits, parity,stopbits);
+	}
+
+	for (i = 0; i < muxdev; ++i) {
+		struct tty_mux_session *tms = session_alloc(i, name_prefix);
+		if (tms) {
+			tms->next = sessions;
+			if (debug_verbose) {
+				printf("%s created\n", tms->name);
+			}
+			ev_io_start(loop, &tms->base);
+			sessions = tms;
+		}
+	}
+
+	ev_run(loop, 0);
+
+	close(uart_fd);
+
+	iniparser_freedict(ini);
+
+	if (muxdev > 0) {
+		struct tty_mux_session *next = sessions;
+		while (sessions) {
+			next = sessions;
+			sessions = sessions->next;
+			next->next = NULL;
+			session_close(next);
 		}
 	}
+
 	return 0;
 }

serialmux.ini

@@ -0,0 +1,12 @@
+[serial]
+device = "/dev/ttyS0"
+baudrate = 115200
+databits = 8
+stopbits = 1
+
+[mux]
+num = 4
+device = "/dev/ttyMUX"
+
+[debug]
+verbose = 0

serialmux.service

@@ -0,0 +1,14 @@
+[Unit]
+Description=Serial Mux Server
+ConditionPathExists=/etc/serialmux.ini
+Before=sysinit.target
+After=systemd-sysusers.service
+
+[Service]
+Type=simple
+Restart=always
+RestartSec=2
+ExecStart=/usr/local/bin/serialmux
+
+[Install]
+WantedBy=basic.target