experiments with demultiplexing PS/2 devices (touchpad + trackpoint) on FreeBSD

psmdemuxd.c

#include <cuse.h>
#include <fcntl.h>
#include <stdio.h>
#include <stdbool.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <sys/mouse.h>
#include <sys/select.h>
#include <sys/poll.h>
#include <sys/param.h>

#define PSM_LEVEL_NATIVE 2

static int psm_fd;
static struct pollfd pollfds[1];

static int vpsm_open(struct cuse_dev *cdev, int fflags) {
	/* bool is_nonblock = true;//fflags & CUSE_FFLAG_NONBLOCK; */
	printf("OPEN %09x\n", fflags);
	if ((psm_fd = open("/dev/psm0", O_RDWR | O_NONBLOCK, 0666)) < 0)
		printf("Could not open psm0! %s\n", strerror(errno));
	ioctl(psm_fd, MOUSE_SETLEVEL, PSM_LEVEL_NATIVE);
	write(psm_fd, "\xFF", 1);
	return CUSE_ERR_NONE;
}

static int vpsm_close(struct cuse_dev *cdev, int fflags) {
	printf("cLOSE\n");
	if (close(psm_fd) != 0)
		printf("Could not close psm0! %s\n", strerror(errno));
	return CUSE_ERR_NONE;
}

static int vpsm_read(struct cuse_dev *cdev, int fflags, void *peer_ptr, int len) {
	if (len < 0)
		return CUSE_ERR_INVALID;

	unsigned char buf[1];
	int from_psm_bytes = read(psm_fd, &buf, 1);
	if (from_psm_bytes == -1) {
		if (errno == EAGAIN)
			return CUSE_ERR_WOULDBLOCK;
		return CUSE_ERR_OTHER;
	}
	/* printf("Got %09x nonbl? %02x | %ld  SYNAP: %08x %08x %08x %08x %08x %08x %08x %08x\n", fflags, fflags & CUSE_FFLAG_NONBLOCK, from_psm_bytes, */
	/* 		buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7]); */
	int err = cuse_copy_out(&buf, peer_ptr, 1);
	if (err)
		return err;
	return 1;
}

static int vpsm_write(struct cuse_dev *cdev, int fflags, const void *peer_ptr, int len) {
	int err;
	unsigned char buf[128];
	bzero(buf, 128);
	err = cuse_copy_in(peer_ptr, buf, len);
	printf("Got %d WRITE: %08x %08x %08x %08x %08x %08x %08x %08x\n", len,
			buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7]);
	write(psm_fd, buf, len);
	if (err)
		return err;
	return len;
}

static int vpsm_ioctl(struct cuse_dev *cdev, int fflags, unsigned long cmd, void *peer_data) {
	unsigned char buf[4096];
	size_t len = IOCPARM_LEN(cmd);
	if ((cmd & IOC_IN) || (cmd & IOC_INOUT))
		cuse_copy_in(peer_data, buf, len);
	printf("IOCTL FFLAGS %08x CMD %016lx LEN %08lx IN %08lx OUT %08lx \n", fflags, cmd, len, cmd & IOC_IN, cmd & IOC_OUT);
	ioctl(psm_fd, cmd, buf);
	if ((cmd & IOC_OUT) || (cmd & IOC_INOUT))
		cuse_copy_out(buf, peer_data, len);
	return CUSE_ERR_NONE;
}

static int vpsm_poll(struct cuse_dev *cdev, int fflags, int events) {
	int revents = 0;
	printf("POLL REQ %09x\n", events);
	if (events & CUSE_POLL_READ) {
		/* printf("POLL RD\n"); */
		pollfds[0].fd = psm_fd;
		pollfds[0].events = POLLIN;
		printf("pollin... %09x", cuse_got_peer_signal());
		int pollres = poll(pollfds, 1, 500);
		printf("... %09x\n", cuse_got_peer_signal());
		if (pollfds[0].revents & POLLIN) {
			revents |= CUSE_POLL_READ;
		}
		if (pollres == 0) {
			revents |= CUSE_POLL_ERROR;
		}
		/* printf("POLL RES %09x\n", pollfds[0].revents); */
	}
	printf("POLL END\n");
	return revents;
}

static struct cuse_methods vpsm_cuse_methods = {
	.cm_open = vpsm_open,
	.cm_close = vpsm_close,
	.cm_read = vpsm_read,
	.cm_write = vpsm_write,
	.cm_ioctl = vpsm_ioctl,
	.cm_poll = vpsm_poll,
};

int main() {
	if (cuse_init() != 0)
		printf("Could not init\n");
	if (cuse_dev_create(&vpsm_cuse_methods, NULL, NULL, 0, 0, 0660, "synapt") != 0)
		printf("Could not create device\n");
	while (true) {
		if (cuse_wait_and_process() != 0)
			break;
	}
	cuse_uninit();
}

psmdemuxd_thr.cpp

#include <thread>
#include <atomic>
#include "readerwriterqueue/readerwriterqueue.h"
extern "C" {
#include <fcntl.h>
#include <stdio.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>
#include <cuse.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <sys/mouse.h>
#include <sys/select.h>
#include <sys/poll.h>
#include <sys/param.h>
}

#define PSM_LEVEL_NATIVE 2

using namespace moodycamel;

static int psm_fd;
static bool reader_present;
static ReaderWriterQueue<uint8_t> byte_queue(8112);

void psm_reader() {
	if ((psm_fd = open("/dev/psm0", O_RDWR | O_NONBLOCK, 0666)) < 0)
		printf("Could not open psm0! %s\n", strerror(errno));
	ioctl(psm_fd, MOUSE_SETLEVEL, PSM_LEVEL_NATIVE);
	write(psm_fd, "\xFF", 1);
	struct pollfd pollfds[1];
	uint8_t cur_byte;
	while (true) {
		pollfds[0].fd = psm_fd;
		pollfds[0].events = POLLIN;
		int pollres = poll(pollfds, 1, INFTIM);
		if (pollfds[0].revents & POLLIN) {
			read(psm_fd, &cur_byte, 1);
			if (reader_present)
				byte_queue.enqueue(cur_byte);
		}
	}
}

static int vpsm_open(struct cuse_dev *cdev, int fflags) {
	printf("OPEN %09x\n", fflags);
	reader_present = true;
	return CUSE_ERR_NONE;
}

static int vpsm_close(struct cuse_dev *cdev, int fflags) {
	reader_present = false;
	return CUSE_ERR_NONE;
}

static int vpsm_read(struct cuse_dev *cdev, int fflags, void *peer_ptr, int len) {
	printf("READ FLAGS %08x\n", fflags);
	uint8_t cur_byte;
	if (!byte_queue.try_dequeue(cur_byte))
		return CUSE_ERR_WOULDBLOCK;
	int err = cuse_copy_out(&cur_byte, peer_ptr, 1);
	if (err)
		return err;
	return 1;
}

static int vpsm_write(struct cuse_dev *cdev, int fflags, const void *peer_ptr, int len) {
	int err;
	unsigned char buf[128];
	bzero(buf, 128);
	err = cuse_copy_in(peer_ptr, buf, len);
	printf("Got %d WRITE: %08x %08x %08x %08x %08x %08x %08x %08x\n", len,
			buf[0], buf[1], buf[2], buf[3], buf[4], buf[5], buf[6], buf[7]);
	write(psm_fd, buf, len);
	if (err)
		return err;
	return len;
}

static int vpsm_ioctl(struct cuse_dev *cdev, int fflags, unsigned long cmd, void *peer_data) {
	unsigned char buf[4096];
	size_t len = IOCPARM_LEN(cmd);
	if ((cmd & IOC_IN) || (cmd & IOC_INOUT))
		cuse_copy_in(peer_data, buf, len);
	printf("IOCTL FFLAGS %08x CMD %016lx LEN %08lx IN %08lx OUT %08lx \n", fflags, cmd, len, cmd & IOC_IN, cmd & IOC_OUT);
	ioctl(psm_fd, cmd, buf);
	if ((cmd & IOC_OUT) || (cmd & IOC_INOUT))
		cuse_copy_out(buf, peer_data, len);
	return CUSE_ERR_NONE;
}

static int vpsm_poll(struct cuse_dev *cdev, int fflags, int events) {
	printf("POLL FLAGS %08x\n", fflags);
	int revents = 0;
	if (events & CUSE_POLL_READ) {
		while (byte_queue.peek() == nullptr) {}
		revents |= CUSE_POLL_READ;
	}
	revents |= CUSE_POLL_WRITE;
	return revents;
}

static struct cuse_methods vpsm_cuse_methods = {
	.cm_open = vpsm_open,
	.cm_close = vpsm_close,
	.cm_read = vpsm_read,
	.cm_write = vpsm_write,
	.cm_ioctl = vpsm_ioctl,
	.cm_poll = vpsm_poll,
};

int main() {
	if (cuse_init() != 0)
		printf("Could not init\n");
	if (cuse_dev_create(&vpsm_cuse_methods, NULL, NULL, 0, 0, 0660, "synapt") != 0)
		printf("Could not create device\n");
	std::thread reader_thr(psm_reader);
	while (true) {
		if (cuse_wait_and_process() != 0)
			break;
	}
	cuse_uninit();
}