From 61716a2c77dfde9addf6b41a6d72d26a8584150e Mon Sep 17 00:00:00 2001 From: Kenny Levinsen Date: Fri, 31 Jul 2020 00:22:18 +0200 Subject: Initial implementation of seatd and libseat --- common/connection.c | 312 ++++++++++++++++++++++++++++++++++++++++++++++++++++ common/drm.c | 23 ++++ common/evdev.c | 15 +++ common/list.c | 76 +++++++++++++ common/log.c | 80 ++++++++++++++ common/terminal.c | 202 ++++++++++++++++++++++++++++++++++ 6 files changed, 708 insertions(+) create mode 100644 common/connection.c create mode 100644 common/drm.c create mode 100644 common/evdev.c create mode 100644 common/list.c create mode 100644 common/log.c create mode 100644 common/terminal.c (limited to 'common') diff --git a/common/connection.c b/common/connection.c new file mode 100644 index 0000000..2545e5d --- /dev/null +++ b/common/connection.c @@ -0,0 +1,312 @@ +#include +#include +#include +#include +#include +#include +#include + +#include "compiler.h" +#include "connection.h" + +#define CLEN (CMSG_LEN(MAX_FDS_OUT * sizeof(int))) + +ALWAYS_INLINE static uint32_t connection_buffer_mask(const uint32_t idx) { + return idx & (CONNECTION_BUFFER_SIZE - 1); +} + +ALWAYS_INLINE static uint32_t connection_buffer_size(const struct connection_buffer *b) { + return b->head - b->tail; +} + +ALWAYS_INLINE static void connection_buffer_consume(struct connection_buffer *b, const size_t size) { + b->tail += size; +} + +ALWAYS_INLINE static void connection_buffer_restore(struct connection_buffer *b, const size_t size) { + b->tail -= size; +} + +/* + * connection_buffer_get_iov prepares I/O vectors pointing to our ring buffer. + * Two may be used if the buffer has wrapped around. + */ +static void connection_buffer_get_iov(struct connection_buffer *b, struct iovec *iov, int *count) { + uint32_t head = connection_buffer_mask(b->head); + uint32_t tail = connection_buffer_mask(b->tail); + if (tail < head) { + iov[0].iov_base = b->data + tail; + iov[0].iov_len = head - tail; + *count = 1; + } else if (head == 0) { + iov[0].iov_base = b->data + tail; + iov[0].iov_len = sizeof b->data - tail; + *count = 1; + } else { + iov[0].iov_base = b->data + tail; + iov[0].iov_len = sizeof b->data - tail; + iov[1].iov_base = b->data; + iov[1].iov_len = head; + *count = 2; + } +} + +/* + * connection_buffer_put_iov prepares I/O vectors pointing to our ring buffer. + * Two may be used if the buffer has wrapped around. + */ +static void connection_buffer_put_iov(struct connection_buffer *b, struct iovec *iov, int *count) { + uint32_t head = connection_buffer_mask(b->head); + uint32_t tail = connection_buffer_mask(b->tail); + if (head < tail) { + iov[0].iov_base = b->data + head; + iov[0].iov_len = tail - head; + *count = 1; + } else if (tail == 0) { + iov[0].iov_base = b->data + head; + iov[0].iov_len = sizeof b->data - head; + *count = 1; + } else { + iov[0].iov_base = b->data + head; + iov[0].iov_len = sizeof b->data - head; + iov[1].iov_base = b->data; + iov[1].iov_len = tail; + *count = 2; + } +} + +/* + * connection_buffer_copy copies from our ring buffer into a linear buffer. + */ +static void connection_buffer_copy(const struct connection_buffer *b, void *data, const size_t count) { + uint32_t tail = connection_buffer_mask(b->tail); + if (tail + count <= sizeof b->data) { + memcpy(data, b->data + tail, count); + return; + } + + uint32_t size = sizeof b->data - tail; + memcpy(data, b->data + tail, size); + memcpy((char *)data + size, b->data, count - size); +} +/* + * connection_buffer_copy copies from a linear buffer into our ring buffer. + */ +static int connection_buffer_put(struct connection_buffer *b, const void *data, const size_t count) { + if (count > sizeof(b->data)) { + errno = EOVERFLOW; + return -1; + } + + uint32_t head = connection_buffer_mask(b->head); + if (head + count <= sizeof b->data) { + memcpy(b->data + head, data, count); + } else { + uint32_t size = sizeof b->data - head; + memcpy(b->data + head, data, size); + memcpy(b->data, (const char *)data + size, count - size); + } + + b->head += count; + return 0; +} + +/* + * close_fds closes all fds within a connection_buffer + */ +static void connection_buffer_close_fds(struct connection_buffer *buffer) { + size_t size = connection_buffer_size(buffer); + if (size == 0) { + return; + } + int fds[sizeof(buffer->data) / sizeof(int)]; + connection_buffer_copy(buffer, fds, size); + int count = size / sizeof fds[0]; + size = count * sizeof fds[0]; + for (int idx = 0; idx < count; idx++) { + close(fds[idx]); + } + connection_buffer_consume(buffer, size); +} + +/* + * build_cmsg prepares a cmsg from a buffer full of fds + */ +static void build_cmsg(struct connection_buffer *buffer, char *data, int *clen) { + size_t size = connection_buffer_size(buffer); + if (size > MAX_FDS_OUT * sizeof(int)) { + size = MAX_FDS_OUT * sizeof(int); + } + + if (size <= 0) { + *clen = 0; + return; + } + + struct cmsghdr *cmsg = (struct cmsghdr *)data; + cmsg->cmsg_level = SOL_SOCKET; + cmsg->cmsg_type = SCM_RIGHTS; + cmsg->cmsg_len = CMSG_LEN(size); + connection_buffer_copy(buffer, CMSG_DATA(cmsg), size); + *clen = cmsg->cmsg_len; +} + +static int decode_cmsg(struct connection_buffer *buffer, struct msghdr *msg) { + bool overflow = false; + struct cmsghdr *cmsg; + for (cmsg = CMSG_FIRSTHDR(msg); cmsg != NULL; cmsg = CMSG_NXTHDR(msg, cmsg)) { + if (cmsg->cmsg_level != SOL_SOCKET || cmsg->cmsg_type != SCM_RIGHTS) { + continue; + } + + size_t size = cmsg->cmsg_len - CMSG_LEN(0); + size_t max = sizeof(buffer->data) - connection_buffer_size(buffer); + if (size > max || overflow) { + overflow = true; + size /= sizeof(int); + for (size_t idx = 0; idx < size; idx++) { + close(((int *)CMSG_DATA(cmsg))[idx]); + } + } else if (connection_buffer_put(buffer, CMSG_DATA(cmsg), size) < 0) { + return -1; + } + } + + if (overflow) { + errno = EOVERFLOW; + return -1; + } + return 0; +} + +int connection_read(struct connection *connection) { + if (connection_buffer_size(&connection->in) >= sizeof(connection->in.data)) { + errno = EOVERFLOW; + return -1; + } + + int count; + struct iovec iov[2]; + connection_buffer_put_iov(&connection->in, iov, &count); + + char cmsg[CLEN]; + struct msghdr msg = { + .msg_name = NULL, + .msg_namelen = 0, + .msg_iov = iov, + .msg_iovlen = count, + .msg_control = cmsg, + .msg_controllen = sizeof cmsg, + .msg_flags = 0, + }; + + int len; + do { + len = recvmsg(connection->fd, &msg, MSG_DONTWAIT | MSG_CMSG_CLOEXEC); + if (len == -1 && errno != EINTR) + return -1; + } while (len == -1); + + if (decode_cmsg(&connection->fds_in, &msg) != 0) { + return -1; + } + connection->in.head += len; + + return connection_buffer_size(&connection->in); +} + +int connection_flush(struct connection *connection) { + if (!connection->want_flush) { + return 0; + } + + uint32_t tail = connection->out.tail; + while (connection->out.head - connection->out.tail > 0) { + int count; + struct iovec iov[2]; + connection_buffer_get_iov(&connection->out, iov, &count); + + int clen; + char cmsg[CLEN]; + build_cmsg(&connection->fds_out, cmsg, &clen); + struct msghdr msg = { + .msg_name = NULL, + .msg_namelen = 0, + .msg_iov = iov, + .msg_iovlen = count, + .msg_control = (clen > 0) ? cmsg : NULL, + .msg_controllen = clen, + .msg_flags = 0, + }; + + int len; + do { + len = sendmsg(connection->fd, &msg, MSG_NOSIGNAL | MSG_DONTWAIT); + if (len == -1 && errno != EINTR) + return -1; + } while (len == -1); + connection_buffer_close_fds(&connection->fds_out); + connection->out.tail += len; + } + connection->want_flush = 0; + return connection->out.head - tail; +} + +int connection_put(struct connection *connection, const void *data, size_t count) { + if (connection_buffer_size(&connection->out) + count > CONNECTION_BUFFER_SIZE) { + connection->want_flush = 1; + if (connection_flush(connection) == -1) { + return -1; + } + } + + if (connection_buffer_put(&connection->out, data, count) == -1) { + return -1; + } + + connection->want_flush = 1; + return 0; +} + +int connection_put_fd(struct connection *connection, int fd) { + if (connection_buffer_size(&connection->fds_out) == MAX_FDS_OUT * sizeof fd) { + errno = EOVERFLOW; + return -1; + } + + return connection_buffer_put(&connection->fds_out, &fd, sizeof fd); +} + +int connection_get(struct connection *connection, void *dst, size_t count) { + if (count > connection_buffer_size(&connection->in)) { + errno = EAGAIN; + return -1; + } + connection_buffer_copy(&connection->in, dst, count); + connection_buffer_consume(&connection->in, count); + return count; +} + +int connection_get_fd(struct connection *connection) { + int fd; + if (sizeof fd > connection_buffer_size(&connection->fds_in)) { + errno = EAGAIN; + return -1; + } + connection_buffer_copy(&connection->fds_in, &fd, sizeof fd); + connection_buffer_consume(&connection->fds_in, sizeof fd); + return fd; +} + +void connection_close_fds(struct connection *connection) { + connection_buffer_close_fds(&connection->fds_in); + connection_buffer_close_fds(&connection->fds_out); +} + +size_t connection_pending(struct connection *connection) { + return connection_buffer_size(&connection->in); +} + +void connection_restore(struct connection *connection, size_t count) { + connection_buffer_restore(&connection->in, count); +} diff --git a/common/drm.c b/common/drm.c new file mode 100644 index 0000000..4b70b64 --- /dev/null +++ b/common/drm.c @@ -0,0 +1,23 @@ +#include +#include +#include + +#include "drm.h" + +// From libdrm +#define DRM_IOCTL_BASE 'd' +#define DRM_IO(nr) _IO(DRM_IOCTL_BASE, nr) +#define DRM_IOCTL_SET_MASTER DRM_IO(0x1e) +#define DRM_IOCTL_DROP_MASTER DRM_IO(0x1f) + +int drm_set_master(int fd) { + return ioctl(fd, DRM_IOCTL_SET_MASTER, 0); +} + +int drm_drop_master(int fd) { + return ioctl(fd, DRM_IOCTL_DROP_MASTER, 0); +} + +int dev_is_drm(dev_t device) { + return major(device) == 226; +} diff --git a/common/evdev.c b/common/evdev.c new file mode 100644 index 0000000..fe6922d --- /dev/null +++ b/common/evdev.c @@ -0,0 +1,15 @@ +#include +#include +#include +#include +#include + +#include "evdev.h" + +int evdev_revoke(int fd) { + return ioctl(fd, EVIOCREVOKE, NULL); +} + +int dev_is_evdev(dev_t device) { + return major(device) == 13; +} diff --git a/common/list.c b/common/list.c new file mode 100644 index 0000000..1a86837 --- /dev/null +++ b/common/list.c @@ -0,0 +1,76 @@ +#include +#include +#include + +#include "list.h" + +void list_init(struct list *list) { + list->capacity = 10; + list->length = 0; + list->items = malloc(sizeof(void *) * list->capacity); +} + +static void list_resize(struct list *list) { + if (list->length == list->capacity) { + list->capacity *= 2; + list->items = realloc(list->items, sizeof(void *) * list->capacity); + } +} + +void list_free(struct list *list) { + list->capacity = 0; + list->length = 0; + free(list->items); +} + +void list_add(struct list *list, void *item) { + list_resize(list); + list->items[list->length++] = item; +} + +void list_insert(struct list *list, size_t index, void *item) { + list_resize(list); + memmove(&list->items[index + 1], &list->items[index], + sizeof(void *) * (list->length - index)); + list->length++; + list->items[index] = item; +} + +void list_del(struct list *list, size_t index) { + list->length--; + memmove(&list->items[index], &list->items[index + 1], + sizeof(void *) * (list->length - index)); +} + +size_t list_find(struct list *list, const void *item) { + for (size_t i = 0; i < list->length; i++) { + if (list->items[i] == item) { + return i; + } + } + return -1; +} + +void list_concat(struct list *list, struct list *source) { + if (list->length + source->length > list->capacity) { + while (list->length + source->length > list->capacity) { + list->capacity *= 2; + } + list->items = realloc(list->items, sizeof(void *) * list->capacity); + } + memmove(&list->items[list->length], source->items, sizeof(void *) * (source->length)); + list->length += source->length; +} + +void list_truncate(struct list *list) { + list->length = 0; +} + +void *list_pop_front(struct list *list) { + if (list->length == 0) { + return NULL; + } + void *item = list->items[0]; + list_del(list, 0); + return item; +} diff --git a/common/log.c b/common/log.c new file mode 100644 index 0000000..1474dcc --- /dev/null +++ b/common/log.c @@ -0,0 +1,80 @@ +#include +#include +#include +#include +#include +#include +#include + +#include "log.h" + +const long NSEC_PER_SEC = 1000000000; + +static enum libseat_log_level current_log_level; +static struct timespec start_time = {-1, -1}; +static bool colored = false; + +static const char *verbosity_colors[] = { + [LIBSEAT_SILENT] = "", + [LIBSEAT_ERROR] = "\x1B[1;31m", + [LIBSEAT_INFO] = "\x1B[1;34m", + [LIBSEAT_DEBUG] = "\x1B[1;90m", +}; + +static const char *verbosity_headers[] = { + [LIBSEAT_SILENT] = "", + [LIBSEAT_ERROR] = "[ERROR]", + [LIBSEAT_INFO] = "[INFO]", + [LIBSEAT_DEBUG] = "[DEBUG]", +}; + +static void timespec_sub(struct timespec *r, const struct timespec *a, const struct timespec *b) { + r->tv_sec = a->tv_sec - b->tv_sec; + r->tv_nsec = a->tv_nsec - b->tv_nsec; + if (r->tv_nsec < 0) { + r->tv_sec--; + r->tv_nsec += NSEC_PER_SEC; + } +} + +void libseat_log_init(enum libseat_log_level level) { + if (start_time.tv_sec >= 0) { + return; + } + clock_gettime(CLOCK_MONOTONIC, &start_time); + current_log_level = level; + colored = isatty(STDERR_FILENO); +} + +void _libseat_logf(enum libseat_log_level level, const char *fmt, ...) { + int stored_errno = errno; + va_list args; + if (level < current_log_level) { + return; + } + + struct timespec ts = {0}; + clock_gettime(CLOCK_MONOTONIC, &ts); + timespec_sub(&ts, &ts, &start_time); + unsigned c = (level < LIBSEAT_LOG_LEVEL_LAST) ? level : LIBSEAT_LOG_LEVEL_LAST - 1; + + const char *prefix, *postfix; + + if (colored) { + prefix = verbosity_colors[c]; + postfix = "\x1B[0m\n"; + } else { + prefix = verbosity_headers[c]; + postfix = "\n"; + } + + fprintf(stderr, "%02d:%02d:%02d.%03ld %s ", (int)(ts.tv_sec / 60 / 60), + (int)(ts.tv_sec / 60 % 60), (int)(ts.tv_sec % 60), ts.tv_nsec / 1000000, prefix); + + va_start(args, fmt); + vfprintf(stderr, fmt, args); + va_end(args); + + fprintf(stderr, "%s", postfix); + errno = stored_errno; +} diff --git a/common/terminal.c b/common/terminal.c new file mode 100644 index 0000000..c9ee758 --- /dev/null +++ b/common/terminal.c @@ -0,0 +1,202 @@ +#include "string.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "log.h" +#include "terminal.h" + +#define TTYPATHLEN 64 + +int terminal_current_vt(void) { + struct vt_stat st; + int fd = open("/dev/tty0", O_RDWR | O_NOCTTY); + if (fd == -1) { + log_errorf("could not open tty0: %s", strerror(errno)); + return -1; + } + int res = ioctl(fd, VT_GETSTATE, &st); + close(fd); + if (res == -1) { + log_errorf("could not retrieve VT state: %s", strerror(errno)); + return -1; + } + return st.v_active; +} + +int terminal_setup(int vt) { + log_debugf("setting up vt %d", vt); + if (vt == -1) { + vt = 0; + } + char path[TTYPATHLEN]; + if (snprintf(path, TTYPATHLEN, "/dev/tty%d", vt) == -1) { + log_errorf("could not generate tty path: %s", strerror(errno)); + return -1; + } + int fd = open(path, O_RDWR | O_NOCTTY); + if (fd == -1) { + log_errorf("could not open target tty: %s", strerror(errno)); + return -1; + } + + static struct vt_mode mode = { + .mode = VT_PROCESS, + .waitv = 0, + .relsig = SIGUSR1, + .acqsig = SIGUSR2, + .frsig = 0, + }; + + int res = ioctl(fd, VT_SETMODE, &mode); + close(fd); + if (res == -1) { + log_errorf("could not set VT mode: %s", strerror(errno)); + } + + return res; +} + +int terminal_teardown(int vt) { + log_debugf("tearing down vt %d", vt); + if (vt == -1) { + vt = 0; + } + char path[TTYPATHLEN]; + if (snprintf(path, TTYPATHLEN, "/dev/tty%d", vt) == -1) { + log_errorf("could not generate tty path: %s", strerror(errno)); + return -1; + } + int fd = open(path, O_RDWR | O_NOCTTY); + if (fd == -1) { + log_errorf("could not open target tty: %s", strerror(errno)); + return -1; + } + if (ioctl(fd, KDSETMODE, KD_TEXT) == -1) { + log_errorf("could not set KD graphics mode: %s", strerror(errno)); + close(fd); + return -1; + } + if (ioctl(fd, KDSKBMODE, K_UNICODE) == -1) { + log_errorf("could not set KD keyboard mode: %s", strerror(errno)); + close(fd); + return -1; + } + + static struct vt_mode mode = { + .mode = VT_PROCESS, + .waitv = 0, + .relsig = SIGUSR1, + .acqsig = SIGUSR2, + .frsig = 0, + }; + if (ioctl(fd, VT_SETMODE, &mode) == -1) { + log_errorf("could not set VT mode: %s", strerror(errno)); + close(fd); + return -1; + } + + close(fd); + return 0; +} + +int terminal_switch_vt(int vt) { + log_debugf("switching to vt %d", vt); + int fd = open("/dev/tty0", O_RDWR | O_NOCTTY); + if (fd == -1) { + log_errorf("could not open tty0: %s", strerror(errno)); + return -1; + } + + static struct vt_mode mode = { + .mode = VT_PROCESS, + .waitv = 0, + .relsig = SIGUSR1, + .acqsig = SIGUSR2, + .frsig = 0, + }; + + if (ioctl(fd, VT_SETMODE, &mode) == -1) { + log_errorf("could not set VT mode: %s", strerror(errno)); + close(fd); + return -1; + } + + if (ioctl(fd, VT_ACTIVATE, vt) == -1) { + log_errorf("could not activate VT: %s", strerror(errno)); + close(fd); + return -1; + } + + close(fd); + return 0; +} + +int terminal_ack_switch(void) { + log_debug("acking vt switch"); + int fd = open("/dev/tty0", O_RDWR | O_NOCTTY); + if (fd == -1) { + log_errorf("could not open tty0: %s", strerror(errno)); + return -1; + } + + int res = ioctl(fd, VT_RELDISP, VT_ACKACQ); + close(fd); + if (res == -1) { + log_errorf("could not ack VT switch: %s", strerror(errno)); + } + + return res; +} + +int terminal_set_keyboard(int vt, bool enable) { + log_debugf("setting KD keyboard state to %d on vt %d", enable, vt); + if (vt == -1) { + vt = 0; + } + char path[TTYPATHLEN]; + if (snprintf(path, TTYPATHLEN, "/dev/tty%d", vt) == -1) { + log_errorf("could not generate tty path: %s", strerror(errno)); + return -1; + } + int fd = open(path, O_RDWR | O_NOCTTY); + if (fd == -1) { + log_errorf("could not generate tty path: %s", strerror(errno)); + return -1; + } + int res = ioctl(fd, KDSKBMODE, enable ? K_UNICODE : K_OFF); + close(fd); + if (res == -1) { + log_errorf("could not set KD keyboard mode: %s", strerror(errno)); + } + return res; +} + +int terminal_set_graphics(int vt, bool enable) { + log_debugf("setting KD graphics state to %d on vt %d", enable, vt); + if (vt == -1) { + vt = 0; + } + char path[TTYPATHLEN]; + if (snprintf(path, TTYPATHLEN, "/dev/tty%d", vt) == -1) { + log_errorf("could not generate tty path: %s", strerror(errno)); + return -1; + } + int fd = open(path, O_RDWR | O_NOCTTY); + if (fd == -1) { + log_errorf("could not generate tty path: %s", strerror(errno)); + return -1; + } + int res = ioctl(fd, KDSETMODE, enable ? KD_GRAPHICS : KD_TEXT); + close(fd); + if (res == -1) { + log_errorf("could not set KD graphics mode: %s", strerror(errno)); + } + return res; +} -- cgit v1.2.3