From ba38bf705ad5798a0ba6600a75fee2e3540da4fc Mon Sep 17 00:00:00 2001 From: viandoxdev Date: Sat, 8 Oct 2022 01:36:53 +0200 Subject: [PATCH] rework ! just need testing --- .clang-format | 3 +- Makefile | 4 +- client.c | 350 ++++++++++++++++++++++--------------- client.h | 18 +- client_config.json | 18 ++ const.c | 4 +- const.h | 3 +- hid.c | 370 +++++++++++++++++++++++---------------- hid.h | 15 +- json.c | 223 ++++++++++++++++++++---- json.h | 71 ++++++-- main.c | 36 ++-- net.c | 168 ++++++++++++++---- net.h | 46 +++-- server.c | 424 +++++++++++++++++++++++++++++++++------------ server.h | 28 ++- server_config.json | 18 ++ util.c | 144 +++++++++++++++ util.h | 21 +++ vec.c | 8 +- vec.h | 3 +- 21 files changed, 1447 insertions(+), 528 deletions(-) create mode 100644 client_config.json create mode 100644 server_config.json diff --git a/.clang-format b/.clang-format index 4ec942d..00b2635 100644 --- a/.clang-format +++ b/.clang-format @@ -3,6 +3,7 @@ BasedOnStyle: LLVM IndentWidth: 4 AlignConsecutiveDeclarations: true AlignConsecutiveAssignments: true +AlignArrayOfStructures: Left PointerAlignment: Right -ColumnLimit: 110 +ColumnLimit: 130 IncludeBlocks: Regroup diff --git a/Makefile b/Makefile index 320d1ae..097a5db 100644 --- a/Makefile +++ b/Makefile @@ -1,11 +1,11 @@ Q=@ CC=gcc -CFLAGS=-g -Wall -Wno-format-truncation -pthread -lm +CFLAGS=-std=c11 -pedantic -g -Wall -Wno-format-truncation -pthread -lm -D_GNU_SOURCE LDFLAGS=-lm BUILD_DIR=./objects BIN=jsfw -RUNARGS=client localhost 7776 +RUNARGS=server 7776 ./server_config.json SOURCES=$(wildcard *.c) diff --git a/client.c b/client.c index c92080f..d99bed4 100644 --- a/client.c +++ b/client.c @@ -1,9 +1,9 @@ #include "client.h" - #include "const.h" #include "json.h" #include "net.h" #include "util.h" +#include "vec.h" #include #include @@ -25,17 +25,10 @@ #include #include -// The current device. -// The fd being -1 means there is none -typedef struct { - int fd; - MessageDeviceInfo info; -} VirtualDevice; - static int fifo_attempt = 0; -static struct sockaddr_in server_addr = {}; -static char server_addrp[64] = {}; +static struct sockaddr_in server_addr = {0}; +static char server_addrp[64] = {0}; static uint16_t server_port = -1; static struct pollfd poll_fds[2]; @@ -44,65 +37,122 @@ static struct pollfd *socket_poll = &poll_fds[1]; static int fifo = -1; static int sock = -1; // static to avoid having this on the stack because a message is about 2kb in memory -static Message message; -static VirtualDevice device = {}; +static Message message; -// Test if the device exists -static inline bool device_exists() { return device.fd > 0; } +static Vec devices_fd; +static Vec devices_info; -// Struct representing the received json -typedef struct { - char *led_color; - double rumble_small; - double rumble_big; - double flash_on; - double flash_off; -} JControllerState; +static ClientConfig config; +static MessageRequest device_request; -static const JSONAdapter JControllerStateAdapter[] = { - {".led_color", String, offsetof(JControllerState, led_color)}, - {".rumble.0", Number, offsetof(JControllerState, rumble_small)}, - {".rumble.1", Number, offsetof(JControllerState, rumble_big)}, - {".flash.0", Number, offsetof(JControllerState, flash_on)}, - {".flash.1", Number, offsetof(JControllerState, flash_off)}, +static void default_fifo_path(void *ptr) { *(char **)ptr = (char *)FIFO_PATH; } +static void default_retry_delay(void *ptr) { *(struct timespec *)ptr = CONNECTION_RETRY_DELAY; } +static void default_vendor(void *ptr) { *(int32_t *)ptr = VIRTUAL_DEVICE_VENDOR; } +static void default_product(void *ptr) { *(int32_t *)ptr = VIRTUAL_DEVICE_PRODUCT; } +static void default_name(void *ptr) { *(char **)ptr = (char *)VIRTUAL_DEVICE_NAME; } +static void default_to_white(void *ptr) { + uint8_t *color = ptr; + color[0] = 255; + color[1] = 255; + color[2] = 255; +} + +static const JSONPropertyAdapter ControllerStateAdapterProps[] = { + {".led_color", &StringAdapter, offsetof(MessageControllerState, led), default_to_white, tsf_hex_to_color }, + {".rumble.0", &NumberAdapter, offsetof(MessageControllerState, small_rumble), default_to_zero_u8, tsf_num_to_u8_clamp}, + {".rumble.1", &NumberAdapter, offsetof(MessageControllerState, big_rumble), default_to_zero_u8, tsf_num_to_u8_clamp}, + {".flash.0", &NumberAdapter, offsetof(MessageControllerState, flash_on), default_to_zero_u8, tsf_num_to_u8_clamp}, + {".flash.1", &NumberAdapter, offsetof(MessageControllerState, flash_off), default_to_zero_u8, tsf_num_to_u8_clamp}, + {".index", &NumberAdapter, offsetof(MessageControllerState, index), default_to_zero_u32, tsf_num_to_int } +}; +static const JSONAdapter ControllerStateAdapter = { + .props = (JSONPropertyAdapter *)ControllerStateAdapterProps, + .prop_count = sizeof(ControllerStateAdapterProps) / sizeof(JSONPropertyAdapter), + .size = sizeof(MessageControllerState), }; -// Try to destroy the device -void device_destroy() { - if (!device_exists()) { +static const JSONPropertyAdapter ControllerAdapterProps[] = { + {".tag", &StringAdapter, offsetof(ClientController, tag), default_to_null, NULL }, + {".vendor", &StringAdapter, offsetof(ClientController, device_vendor), default_vendor, tsf_hex_to_i32}, + {".product", &StringAdapter, offsetof(ClientController, device_product), default_product, tsf_hex_to_i32}, + {".name", &StringAdapter, offsetof(ClientController, device_name), default_name, NULL }, +}; +static const JSONAdapter ControllerAdapter = { + .props = ControllerAdapterProps, + .prop_count = sizeof(ControllerAdapterProps) / sizeof(JSONPropertyAdapter), + .size = sizeof(ClientController), +}; + +static const JSONPropertyAdapter ClientConfigAdapterProps[] = { + {".controllers[]", &ControllerAdapter, offsetof(ClientConfig, controllers), default_to_null, NULL }, + {".fifo_path", &StringAdapter, offsetof(ClientConfig, fifo_path), default_fifo_path, NULL }, + {".retry_delay", &NumberAdapter, offsetof(ClientConfig, retry_delay), default_retry_delay, tsf_numsec_to_timespec} +}; +static const JSONAdapter ConfigAdapter = { + .props = ClientConfigAdapterProps, + .prop_count = sizeof(ClientConfigAdapterProps) / sizeof(JSONPropertyAdapter), + .size = sizeof(ClientConfig), +}; + +void destroy_devices() { + for (int i = 0; i < config.controller_count; i++) { + int fd = *(int *)vec_get(&devices_fd, i); + MessageDeviceInfo *info = vec_get(&devices_info, i); + + if (info->code == DeviceInfo) { + ioctl(fd, UI_DEV_DESTROY); + info->code = NoMessage; + } + } +} + +bool device_exists(int index) { + if (index >= devices_info.len) { + return false; + } + + MessageDeviceInfo *info = vec_get(&devices_info, index); + return info->code == DeviceInfo; +} + +void device_destroy(int index) { + if (index >= devices_info.len) { return; } - ioctl(device.fd, UI_DEV_DESTROY); - close(device.fd); - device.fd = -1; - printf("CLIENT: Destroyed device\n"); + int fd = *(int *)vec_get(&devices_fd, index); + + MessageDeviceInfo *info = vec_get(&devices_info, index); + + if (info->code == DeviceInfo) { + ioctl(fd, UI_DEV_DESTROY); + info->code = NoMessage; + } } -// (Re)Initialize the device void device_init(MessageDeviceInfo *dev) { - device_destroy(); - - int fd = open("/dev/uinput", O_WRONLY | O_NONBLOCK); - if (fd < 0) { - perror("CLIENT: Error while opening /dev/uinput, "); - exit(1); + if (dev->index >= devices_info.len) { + printf("CLIENT: Got wrong device index\n"); + return; } - // Setup device_info + device_destroy(dev->index); + + int fd = *(int*)vec_get(&devices_fd, dev->index); // Abs if (dev->abs_count > 0) { ioctl(fd, UI_SET_EVBIT, EV_ABS); for (int i = 0; i < dev->abs_count; i++) { - struct uinput_abs_setup setup = {}; - setup.code = dev->abs_id[i]; - setup.absinfo.minimum = dev->abs_min[i]; - setup.absinfo.maximum = dev->abs_max[i]; - setup.absinfo.fuzz = dev->abs_fuzz[i]; - setup.absinfo.flat = dev->abs_flat[i]; - setup.absinfo.resolution = dev->abs_res[i]; - setup.absinfo.value = 0; + struct uinput_abs_setup setup = {0}; + + setup.code = dev->abs_id[i]; + setup.absinfo.minimum = dev->abs_min[i]; + setup.absinfo.maximum = dev->abs_max[i]; + setup.absinfo.fuzz = dev->abs_fuzz[i]; + setup.absinfo.flat = dev->abs_flat[i]; + setup.absinfo.resolution = dev->abs_res[i]; + setup.absinfo.value = 0; ioctl(fd, UI_ABS_SETUP, &setup); } } @@ -123,67 +173,95 @@ void device_init(MessageDeviceInfo *dev) { } } - struct uinput_setup setup = {}; + ClientController *ctr = &config.controllers[dev->index]; + + struct uinput_setup setup = {0}; setup.id.bustype = BUS_VIRTUAL; - setup.id.vendor = VIRTUAL_DEVICE_VENDOR; - setup.id.product = VIRTUAL_DEVICE_PRODUCT; + setup.id.vendor = ctr->device_vendor; + setup.id.product = ctr->device_product; setup.id.version = VIRTUAL_DEVICE_VERSION; - strncpy(setup.name, VIRTUAL_DEVICE_NAME, UINPUT_MAX_NAME_SIZE); + strncpy(setup.name, ctr->device_name, UINPUT_MAX_NAME_SIZE); ioctl(fd, UI_DEV_SETUP, &setup); ioctl(fd, UI_DEV_CREATE); - device.fd = fd; - memcpy(&device.info, dev, sizeof(MessageDeviceInfo)); - printf("CLIENT: Created device (abs: %d, rel: %d, key: %d)\n", dev->abs_count, dev->rel_count, - dev->key_count); + MessageDeviceInfo * dst = vec_get(&devices_info, dev->index); + + memcpy(dst, dev, sizeof(MessageDeviceInfo)); + printf("CLIENT: Got device [%d]: '%s' (abs: %d, rel: %d, key: %d)\n", dev->index, ctr->device_name, dev->abs_count, dev->rel_count, dev->key_count); } // Send an event to uinput, device must exist -int device_emit(uint16_t type, uint16_t id, uint32_t value) { - struct input_event event = {}; +bool device_emit(int index, uint16_t type, uint16_t id, uint32_t value) { + if(index >= devices_fd.len) { + return true; + } + + int fd = *(int*) vec_get(&devices_fd, index); + struct input_event event = {0}; event.type = type; event.code = id; event.value = value; - return write(device.fd, &event, sizeof(event)) != sizeof(event); + return write(fd, &event, sizeof(event)) != sizeof(event); } // Update device with report void device_handle_report(MessageDeviceReport *report) { - if (!device_exists()) { - printf("CLIENT: Got report before device info\n"); + if (!device_exists(report->index)) { + printf("CLIENT: [%d] Got report before device info\n", report->index); return; } - if (report->abs_count != device.info.abs_count || report->rel_count != device.info.rel_count || - report->key_count != device.info.key_count) { + MessageDeviceInfo * info = vec_get(&devices_info, report->index); + + if (report->abs_count != info->abs_count || report->rel_count != info->rel_count || + report->key_count != info->key_count) { printf("CLIENT: Report doesn't match with device info\n"); return; } for (int i = 0; i < report->abs_count; i++) { - if (device_emit(EV_ABS, device.info.abs_id[i], report->abs[i]) != 0) { + if (device_emit(report->index, EV_ABS, info->abs_id[i], report->abs[i]) != 0) { printf("CLIENT: Error writing abs event to uinput\n"); } } for (int i = 0; i < report->rel_count; i++) { - if (device_emit(EV_REL, device.info.rel_id[i], report->rel[i]) != 0) { + if (device_emit(report->index, EV_REL, info->rel_id[i], report->rel[i]) != 0) { printf("CLIENT: Error writing rel event to uinput\n"); } } for (int i = 0; i < report->key_count; i++) { - if (device_emit(EV_KEY, device.info.key_id[i], (uint32_t)(!report->key[i]) - 1) != 0) { + if (device_emit(report->index, EV_KEY, info->key_id[i], (uint32_t)(!report->key[i]) - 1) != 0) { printf("CLIENT: Error writing key event to uinput\n"); } } // Reports are sent by the server every time the server receives an EV_SYN from the physical device, so we // send one when we receive the report to match - device_emit(EV_SYN, 0, 0); + device_emit(report->index, EV_SYN, 0, 0); +} + +void setup_devices() { + devices_fd = vec_of(int); + devices_info = vec_of(MessageDeviceInfo); + + MessageDeviceInfo no_info = {0}; + no_info.code = NoMessage; + + for (int i = 0; i < config.controller_count; i++) { + int fd = open("/dev/uinput", O_WRONLY | O_NONBLOCK); + if (fd < 0) { + perror("CLIENT: Can't open /dev/uinput, aborting now"); + exit(1); + } + + vec_push(&devices_fd, &fd); + vec_push(&devices_info, &no_info); + } } void setup_fifo(); @@ -191,10 +269,10 @@ void setup_fifo(); // (Re)Open the fifo void open_fifo() { close(fifo); - fifo = open(FIFO_PATH, O_RDONLY | O_NONBLOCK); + fifo = open(config.fifo_path, O_RDONLY | O_NONBLOCK); if (fifo < 0 && fifo_attempt == 0) { fifo_attempt++; - unlink(FIFO_PATH); + unlink(config.fifo_path); setup_fifo(); } else if (fifo < 0) { panicf("CLIENT: Couldn't open fifo, aborting\n"); @@ -205,7 +283,7 @@ void open_fifo() { // Ensure the fifo exists and opens it (also setup poll_fd) void setup_fifo() { mode_t prev = umask(0); - mkfifo(FIFO_PATH, 0666); + mkfifo(config.fifo_path, 0666); umask(prev); open_fifo(); @@ -216,11 +294,11 @@ void setup_fifo() { // (Re)Connect to the server void connect_server() { - while (1) { + while (true) { if (sock > 0) { // Close previous connection - device_destroy(); shutdown(sock, SHUT_RDWR); + destroy_devices(); close(sock); } @@ -230,11 +308,9 @@ void connect_server() { } if (connect(sock, (struct sockaddr *)&server_addr, sizeof(server_addr)) != 0) { - printf("CLIENT: Couldn't connect to %s:%d, retrying in %ds\n", server_addrp, server_port, - CONNECTION_RETRY_DELAY); - struct timespec ts = {}; - ts.tv_sec = CONNECTION_RETRY_DELAY; - nanosleep(&ts, NULL); + printf("CLIENT: Couldn't connect to %s:%d, retrying in %lu.%09lus\n", server_addrp, server_port, + config.retry_delay.tv_sec, config.retry_delay.tv_nsec); + nanosleep(&config.retry_delay, NULL); continue; } // Set non blocking, only do that after connection (instead of with SOCK_NONBLOCK at socket creation) @@ -242,6 +318,16 @@ void connect_server() { fcntl(sock, F_SETFL, fcntl(sock, F_GETFL, 0) | O_NONBLOCK); socket_poll->fd = sock; printf("CLIENT: Connected !\n"); + + uint8_t buf[2048] __attribute__((aligned(4))); + + int len = msg_serialize(buf, 2048, (Message *)&device_request); + if (len > 0) { + if (send(sock, buf, len, 0) > 0) { + printf("CLIENT: Sent device request\n"); + }; + }; + return; } } @@ -264,22 +350,45 @@ void setup_server(char *address, uint16_t port) { connect_server(); } -void early_checks() { - int fd = open("/dev/uinput", O_WRONLY | O_NONBLOCK); - if (fd < 0) { - perror("CLIENT: Can't open /dev/uinput, aborting now: "); - exit(1); +void build_device_request() { + char **tags = malloc(config.controller_count * sizeof(char *)); + for (int i = 0; i < config.controller_count; i++) { + tags[i] = config.controllers[i].tag; } - close(fd); + + device_request.code = Request; + device_request.request_count = config.controller_count; + device_request.requests = tags; } -void client_run(char *address, uint16_t port) { - // Device doesn't exist yet - device.fd = -1; +void client_run(char *address, uint16_t port, char *config_path) { + // Parse the config + { + FILE *configfd = fopen(config_path, "r"); + if (configfd == NULL) { + perror("CLIENT: Couldn't open config file"); + exit(1); + } + + char *cbuf = malloc(8192); + uint8_t *jbuf = (uint8_t *)cbuf + 4096; + + int len = fread(cbuf, 1, 4096, configfd); + if (json_parse(cbuf, len, jbuf, 4096) != 0) { + printf("CLIENT: Couldn't parse config, %s (at index %lu)\n", json_strerr(), json_errloc()); + exit(1); + } + + json_adapt(jbuf, &ConfigAdapter, &config); + + free(cbuf); + fclose(configfd); + } - early_checks(); setup_fifo(); + build_device_request(); setup_server(address, port); + setup_devices(); uint8_t buf[2048] __attribute__((aligned(4))); uint8_t json_buf[2048] __attribute__((aligned(8))); @@ -287,7 +396,7 @@ void client_run(char *address, uint16_t port) { while (1) { int rc = poll(poll_fds, 2, -1); if (rc < 0) { - perror("CLIENT: Error on poll, "); + perror("CLIENT: Error on poll"); exit(1); } @@ -299,55 +408,19 @@ void client_run(char *address, uint16_t port) { // We've got data from the fifo int rc = json_parse((char *)buf, len, json_buf, 2048); if (rc < 0) { - printf("CLIENT: Error when parsing fifo message as json (%s at index %lu)\n", - json_strerr(), json_errloc()); + printf("CLIENT: Error when parsing fifo message as json (%s at index %lu)\n", json_strerr(), json_errloc()); } else { - JControllerState state; - // default values - state.flash_off = 0.0; - state.flash_on = 0.0; - state.led_color = NULL; - state.rumble_small = 0.0; - state.rumble_big = 0.0; - - json_adapt(json_buf, (JSONAdapter *)JControllerStateAdapter, - sizeof(JControllerStateAdapter) / sizeof(JSONAdapter), &state); - MessageControllerState msg; - msg.code = ControllerState; - msg.small_rumble = (uint8_t)(fmax(fmin(1.0, state.rumble_small), 0.0) * 255.0); - msg.big_rumble = (uint8_t)(fmax(fmin(1.0, state.rumble_big), 0.0) * 255.0); - msg.flash_on = (uint8_t)(fmax(fmin(1.0, state.flash_on), 0.0) * 255.0); - msg.flash_off = (uint8_t)(fmax(fmin(1.0, state.flash_off), 0.0) * 255.0); - - if (state.led_color == NULL || strnlen(state.led_color, 8) != 7) { - msg.led[0] = 0; - msg.led[1] = 0; - msg.led[2] = 0; - } else { - char *s = state.led_color; - msg.led[0] = parse_hex_digit(s[1]); - msg.led[0] <<= 4; - msg.led[0] += parse_hex_digit(s[2]); - - msg.led[1] = parse_hex_digit(s[3]); - msg.led[1] <<= 4; - msg.led[1] += parse_hex_digit(s[4]); - - msg.led[2] = parse_hex_digit(s[5]); - msg.led[2] <<= 4; - msg.led[2] += parse_hex_digit(s[6]); - - free(state.led_color); - } + msg.code = ControllerState; + json_adapt(json_buf, &ControllerStateAdapter, &msg); int len = msg_serialize(buf, 2048, (Message *)&msg); if (len > 0) { if (send(sock, buf, len, 0) > 0) { printf("CLIENT: Sent controller state: #%02x%02x%02x flash: (%d, %d) rumble: " - "(%d, %d)\n", - msg.led[0], msg.led[1], msg.led[2], msg.flash_on, msg.flash_off, - msg.small_rumble, msg.big_rumble); + "(%d, %d) -> [%d]\n", + msg.led[0], msg.led[1], msg.led[2], msg.flash_on, msg.flash_off, msg.small_rumble, + msg.big_rumble, msg.index); }; }; } @@ -359,9 +432,8 @@ void client_run(char *address, uint16_t port) { int len = recv(sock, buf, 2048, 0); if (len <= 0) { printf("CLIENT: Lost connection to server, reconnecting\n"); - shutdown(sock, SHUT_RDWR); connect_server(); - // we can continue here because there's nothing after, unlike above for fifo (this reduces + // we can use continue here because there's nothing after, unlike above for fifo (this reduces // indentation instead of needing an else block) continue; } @@ -384,13 +456,17 @@ void client_run(char *address, uint16_t port) { } if (message.code == DeviceInfo) { - if (device_exists()) { - printf("CLIENT: Got more than one device info\n"); + if (device_exists(message.device_info.index)) { + printf("CLIENT: Got more than one device info for same device\n"); } device_init((MessageDeviceInfo *)&message); + printf("CLIENT: Got device %d\n", message.device_info.index); } else if (message.code == DeviceReport) { device_handle_report((MessageDeviceReport *)&message); + } else if (message.code == DeviceDestroy) { + device_destroy(message.destroy.index); + printf("CLIENT: Lost device %d\n", message.destroy.index); } else { printf("CLIENT: Illegal message\n"); } diff --git a/client.h b/client.h index 9e069af..ba7e7ce 100644 --- a/client.h +++ b/client.h @@ -2,7 +2,23 @@ #ifndef CLIENT_H_ #define CLIENT_H_ #include +#include -void client_run(char *address, uint16_t port); +void client_run(char *address, uint16_t port, char *config_path); + +typedef struct { + char *tag; + int32_t device_vendor; + int32_t device_product; + char *device_name; +} ClientController; + +typedef struct { + ClientController *controllers; + size_t controller_count; + + char *fifo_path; + struct timespec retry_delay; +} ClientConfig; #endif diff --git a/client_config.json b/client_config.json new file mode 100644 index 0000000..f86d9b0 --- /dev/null +++ b/client_config.json @@ -0,0 +1,18 @@ +{ + "controllers": [ + { + "tag": "Admin", + "vendor": "054c", + "product": "09cc", + "name": "JSFW PS4 Controller (Admin)" + }, + { + "tag": "User", + "vendor": "6942", + "product": "1337", + "name": "JSFW PS4 Controller (User)" + } + ], + "fifo_path": "/tmp/jsfw_fifo_2", + "retry_delay": 2.5 +} diff --git a/const.c b/const.c index ee2885c..3fbd0e9 100644 --- a/const.c +++ b/const.c @@ -5,12 +5,14 @@ // How long between each device poll const struct timespec POLL_DEVICE_INTERVAL = {.tv_sec = 1, .tv_nsec = 0}; +// How long (in ms) to wait for a request message on a connection before giving up +const int REQUEST_TIMEOUT = 2000; // Default name for physical device, only visible in logs const char *DEVICE_DEFAULT_NAME = "Unnamed Device"; // Path to the fifo const char *FIFO_PATH = "/tmp/jsfw_fifo"; // Delay (in seconds) between each connection retry for the client -const uint32_t CONNECTION_RETRY_DELAY = 5; +const struct timespec CONNECTION_RETRY_DELAY = {.tv_sec = 5, .tv_nsec = 0}; // Displayed vendor for the virtual device const uint16_t VIRTUAL_DEVICE_VENDOR = 0x6969; // Displayed product for the virtual device diff --git a/const.h b/const.h index 260d5a8..93e9b67 100644 --- a/const.h +++ b/const.h @@ -5,9 +5,10 @@ #include extern const struct timespec POLL_DEVICE_INTERVAL; +extern const int REQUEST_TIMEOUT; extern const char *DEVICE_DEFAULT_NAME; extern const char *FIFO_PATH; -extern const uint32_t CONNECTION_RETRY_DELAY; +extern const struct timespec CONNECTION_RETRY_DELAY; extern const uint16_t VIRTUAL_DEVICE_VENDOR; extern const uint16_t VIRTUAL_DEVICE_PRODUCT; extern const uint16_t VIRTUAL_DEVICE_VERSION; diff --git a/hid.c b/hid.c index 0384e28..4327d8f 100644 --- a/hid.c +++ b/hid.c @@ -1,6 +1,7 @@ #include "hid.h" #include "const.h" +#include "server.h" #include "util.h" #include "vec.h" @@ -18,17 +19,19 @@ #include // List of uniq of the currently known devices -static Vec devices; -// List of the new devices of a poll, static to keep the allocation alive -static Vec new_devices; -// Queue of devices to be taken by connections -static Vec devices_queue; -// Mutex for the device queue -static pthread_mutex_t devices_queue_mutex = PTHREAD_MUTEX_INITIALIZER; -// Condvar notified on device queue update -static pthread_cond_t devices_queue_cond = PTHREAD_COND_INITIALIZER; +static Vec known_devices; +// Queue of available devices, devices that can only be given to one client +static Vec available_devices; +// List of cloneable devices, devices that can be handed out to multiple clients +static Vec cloneable_devices; // Mutex for devices static pthread_mutex_t devices_mutex = PTHREAD_MUTEX_INITIALIZER; +// Condvar notified on devices update +static pthread_cond_t devices_cond = PTHREAD_COND_INITIALIZER; +// Mutex for devices +static pthread_mutex_t known_devices_mutex = PTHREAD_MUTEX_INITIALIZER; + +static ServerConfig *config; // uniqs are just hexadecimal numbers with colons in between each byte uniq_t parse_uniq(char uniq[17]) { @@ -64,8 +67,8 @@ void setup_device(PhysicalDevice *dev) { for (int i = 0; i < KEY_CNT; i++) dev->mapping.key_indices[i] = -1; - uint8_t type_bits[EV_MAX] = {}; - uint8_t feat_bits[(KEY_MAX + 7) / 8] = {}; + uint8_t type_bits[EV_MAX] = {0}; + uint8_t feat_bits[(KEY_MAX + 7) / 8] = {0}; ioctl(dev->event, EVIOCGBIT(0, EV_MAX), type_bits); // Loop over all event types @@ -116,13 +119,8 @@ void setup_device(PhysicalDevice *dev) { } } -// Function used to filter out devices that we don't want. -// This is pretty arbritrary -bool filter_event(int fd, char *event) { - // Check for existance of a js* directory in /sys/class/input/eventXX/device - // This is used to filter out the touchpad of PS4 controller (which have the same product and vendor id as - // the controller) - { +bool filter_event(int fd, char *event, ControllerFilter *filter) { + if (filter->js) { char device_path[64]; snprintf(device_path, 64, "/sys/class/input/%s/device", event); @@ -144,85 +142,112 @@ bool filter_event(int fd, char *event) { } } - // Check product and vendor id 054c:05c4 => Dualshock 4 (09cc is for the second generation) - uint16_t info[4]; - ioctl(fd, EVIOCGID, info); - return info[1] == 0x054c && (info[2] == 0x05c4 || info[2] == 0x09cc); + struct input_id ids; + ioctl(fd, EVIOCGID, &ids); + + if (filter->vendor > 0 && filter->vendor != ids.vendor) + return false; + if (filter->product > 0 && filter->product != ids.product) + return false; + + return true; } // Initialize vectors for polling void poll_devices_init() { - devices = vec_of(uniq_t); - new_devices = vec_of(PhysicalDevice); - devices_queue = vec_of(PhysicalDevice); + known_devices = vec_of(Controller); + cloneable_devices = vec_of(Controller *); + available_devices = vec_of(Controller *); } // Block to get a device, this is thread safe -PhysicalDevice get_device() { +Controller *get_device(char *tag) { // Check if we can get one right away - pthread_mutex_lock(&devices_queue_mutex); - if (devices_queue.len > 0) { - PhysicalDevice r; - vec_pop(&devices_queue, &r); - pthread_mutex_unlock(&devices_queue_mutex); + pthread_mutex_lock(&devices_mutex); - return r; + while (1) { + for (int i = 0; i < available_devices.len; i++) { + Controller *c = *(Controller **)vec_get(&available_devices, i); + if (strcmp(c->ctr.tag, tag) == 0) { + vec_remove(&available_devices, i, NULL); + pthread_mutex_unlock(&devices_mutex); + return c; + } + } + + for (int i = 0; i < cloneable_devices.len; i++) { + Controller *c = *(Controller **)vec_get(&cloneable_devices, i); + if (strcmp(c->ctr.tag, tag) == 0) { + pthread_mutex_unlock(&devices_mutex); + return c; + } + } + + // Wait on condvar until there's a device and we can unlock the mutex + pthread_cond_wait(&devices_cond, &devices_mutex); } - // Wait on condvar until there's a device and we can unlock the mutex - while (devices_queue.len == 0) { - pthread_cond_wait(&devices_queue_cond, &devices_queue_mutex); - } - - // Take a device from the queue - PhysicalDevice res; - vec_pop(&devices_queue, &res); - - // Signal another thread if there are still device(s) left in the queue - if (devices_queue.len > 0) { - pthread_cond_signal(&devices_queue_cond); - } - - pthread_mutex_unlock(&devices_queue_mutex); - return res; } -// Forget about a device. This is used on two cases: -// - If the connection to a client is lost, the device is forgotten, picked up by the next device poll, and -// put back in the queue -// - If the device dies (i.e unplugged), the connection to the client is closed and the device forgotten. -// -// This is thread safe -void return_device(PhysicalDevice *dev) { - if (dev->name != NULL && dev->name != DEVICE_DEFAULT_NAME) { - // Free the name if it was allocated - printf("HID: Returning device '%s' (%012lx)\n", dev->name, dev->uniq); - free(dev->name); - } else { - printf("HID: Returning device %012lx\n", dev->uniq); +// Return a device that isn't used anymore, this really only makes sense for non cloneable devices. +void return_device(Controller *c) { + // If device is cloneable there is nothing to return + if (c->ctr.duplicate) { + return; } + + pthread_mutex_lock(&devices_mutex); + vec_push(&available_devices, &c); + // Signal that there are new devices + pthread_cond_broadcast(&devices_cond); + pthread_mutex_unlock(&devices_mutex); +} + +// Forget about a broken device. This invalidates the reference to the controller +void forget_device(Controller *c) { + + // If controller is cloneable we need to remove it from the cloneable list + if (c->ctr.duplicate) { + pthread_mutex_lock(&devices_mutex); + for (int i = 0; i < cloneable_devices.len; i++) { + Controller *d = *(Controller **)vec_get(&cloneable_devices, i); + if (d->dev.uniq == c->dev.uniq) { + vec_remove(&cloneable_devices, i, NULL); + break; + } + } + pthread_mutex_unlock(&devices_mutex); + } + + // Free the name if it was allocated + if (c->dev.name != NULL && c->dev.name != DEVICE_DEFAULT_NAME) { + printf("HID: Forgetting device '%s' (%012lx)\n", c->dev.name, c->dev.uniq); + free(c->dev.name); + } else { + printf("HID: Forgetting device %012lx\n", c->dev.uniq); + } + // try to close the file descriptor, they may be already closed if the device was unpugged. - close(dev->event); - close(dev->hidraw); + close(c->dev.event); + close(c->dev.hidraw); // Safely remove device from the known device list - pthread_mutex_lock(&devices_mutex); - for (int i = 0; i < devices.len; i++) { - uniq_t *uniq = vec_get(&devices, i); - if (*uniq == dev->uniq) { - vec_remove(&devices, i, NULL); + pthread_mutex_lock(&known_devices_mutex); + for (int i = 0; i < known_devices.len; i++) { + Controller *d = vec_get(&known_devices, i); + if (d->dev.uniq == c->dev.uniq) { + vec_remove(&known_devices, i, NULL); break; } } - pthread_mutex_unlock(&devices_mutex); + pthread_mutex_unlock(&known_devices_mutex); } // Find all available devices and pick up on new ones void poll_devices() { - vec_clear(&new_devices); - // loop over all entries of /sys/class/input DIR *input_dir = opendir("/sys/class/input"); struct dirent *input; + while ((input = readdir(input_dir)) != NULL) { // Ignore if the entry isn't a link or doesn't start with event if (input->d_type != DT_LNK || strncmp(input->d_name, "event", 5) != 0) { @@ -230,93 +255,117 @@ void poll_devices() { } PhysicalDevice dev; + dev.hidraw = -1; // Open /dev/input/eventXX - char event_path[64]; - snprintf(event_path, 64, "/dev/input/%s", input->d_name); + { + char event_path[64]; + snprintf(event_path, 64, "/dev/input/%s", input->d_name); - dev.event = open(event_path, O_RDONLY); + dev.event = open(event_path, O_RDONLY); - if (dev.event < 0) { // Ignore device if we couldn't open - continue; + if (dev.event < 0) { // Ignore device if we couldn't open + continue; + } } // Try to get the name, default to DEFAULT_NAME if impossible - char name_buf[256] = {}; - const char *name; - if (ioctl(dev.event, EVIOCGNAME(256), name_buf) >= 0) { - name = name_buf; - } else { - name = DEVICE_DEFAULT_NAME; + char *name; + { + static char name_buf[256] = {0}; + if (ioctl(dev.event, EVIOCGNAME(256), name_buf) >= 0) { + name = name_buf; + } else { + name = (char *)DEVICE_DEFAULT_NAME; + } } - // Filter events we don't care about - if (!filter_event(dev.event, input->d_name)) { - goto skip; + // Used for linear searches + bool found; + + // Filter devices according server config + ServerConfigController *ctr; + { + found = false; + for (int i = 0; i < config->controller_count; i++) { + ctr = &config->controllers[i]; + + if (filter_event(dev.event, input->d_name, &ctr->filter)) { + found = true; + break; + } + } + + if (!found) { + goto skip; + } } // Try to get uniq, drop device if we can't - uniq_t uniq; { - char uniq_str[17] = {}; + char uniq_str[17] = {0}; ioctl(dev.event, EVIOCGUNIQ(17), uniq_str); - uniq = parse_uniq(uniq_str); + dev.uniq = parse_uniq(uniq_str); // If we couldn't parse the uniq (this assumes uniq can't be zero, which is probably alright) - if (uniq == 0) { + if (dev.uniq == 0) { goto skip; } } // Check if we already know of this device - bool found = false; - - pthread_mutex_lock(&devices_mutex); - for (int i = 0; i < devices.len; i++) { - uniq_t *dev_uniq = vec_get(&devices, i); - if (*dev_uniq == uniq) { - found = true; - break; - } - } - pthread_mutex_unlock(&devices_mutex); - - if (found) { // Device isn't new - goto skip; - } - - dev.uniq = uniq; - - // Try to find hidraw path for the device, drop the device if we can't - char hidraw_path[64]; { - char hidraw_dir_path[256]; - snprintf(hidraw_dir_path, 256, "/sys/class/input/%s/device/device/hidraw", input->d_name); + found = false; - DIR *hidraw_dir = opendir(hidraw_dir_path); - struct dirent *hidraw = NULL; - while ((hidraw = readdir(hidraw_dir)) != NULL) { - if (strncmp(hidraw->d_name, "hidraw", 6) == 0) { + pthread_mutex_lock(&known_devices_mutex); + for (int i = 0; i < known_devices.len; i++) { + Controller *c = vec_get(&known_devices, i); + if (c->dev.uniq == dev.uniq) { + found = true; break; } } + pthread_mutex_unlock(&known_devices_mutex); - if (hidraw == NULL) { - printf("Couldn't get hidraw of %s", input->d_name); - continue; + if (found) { // Device isn't new + goto skip; } - - snprintf(hidraw_path, 64, "/dev/%s", hidraw->d_name); - - closedir(hidraw_dir); } - dev.hidraw = open(hidraw_path, O_WRONLY); - if (dev.hidraw < 0) { - goto skip; + // Look for hidraw if the device should have one (Dualshock 4 only, with ps4_hidraw property set) + if (ctr->ps4_hidraw) { + // Attempt to find the path + char hidraw_path[64]; + { + char hidraw_dir_path[256]; + snprintf(hidraw_dir_path, 256, "/sys/class/input/%s/device/device/hidraw", input->d_name); + + DIR *hidraw_dir = opendir(hidraw_dir_path); + struct dirent *hidraw = NULL; + while ((hidraw = readdir(hidraw_dir)) != NULL) { + if (strncmp(hidraw->d_name, "hidraw", 6) == 0) { + break; + } + } + + if (hidraw == NULL) { + printf("HID: Couldn't get hidraw of %s", input->d_name); + goto skip; + } + + snprintf(hidraw_path, 64, "/dev/%s", hidraw->d_name); + + closedir(hidraw_dir); + } + // Try to open + dev.hidraw = open(hidraw_path, O_WRONLY); + if (dev.hidraw < 0) { + goto skip; + } } + // Allocate for name (only now to avoid unecessary allocations) if (name != DEVICE_DEFAULT_NAME) { dev.name = malloc(256); @@ -327,40 +376,55 @@ void poll_devices() { } } - setup_device(&dev); + // This code is only run if the device has passed all filters and requirements + { + setup_device(&dev); + Controller c = {.dev = dev, .ctr = *ctr}; - pthread_mutex_lock(&devices_mutex); - vec_push(&devices, &uniq); - pthread_mutex_unlock(&devices_mutex); + pthread_mutex_lock(&known_devices_mutex); + // Index of the device in known_devices + int index = known_devices.len; + vec_push(&known_devices, &c); + pthread_mutex_unlock(&known_devices_mutex); - vec_push(&new_devices, &dev); + // Pointer to the device in known_devices + Controller *p = vec_get(&known_devices, index); - printf("HID: New device, %s (%s: %012lx)\n", name, input->d_name, dev.uniq); - // Continue here to avoid running cleanup code of skip + printf("HID: New device, %s [%s] (%s: %012lx)\n", name, ctr->tag, input->d_name, dev.uniq); + + if (ctr->duplicate) { + pthread_mutex_lock(&devices_mutex); + vec_push(&cloneable_devices, &p); + // Signal that there are new cloneable devices + pthread_cond_broadcast(&devices_cond); + pthread_mutex_unlock(&devices_mutex); + } else { + pthread_mutex_lock(&devices_mutex); + vec_push(&available_devices, &p); + // Signal that there are new devices + pthread_cond_broadcast(&devices_cond); + pthread_mutex_unlock(&devices_mutex); + } + } + // Continue here avoids running cleanup code continue; // close open file descriptor and continue skip: close(dev.event); - continue; }; closedir(input_dir); - - // Safely add new devices to the queue - if (new_devices.len > 0) { - pthread_mutex_lock(&devices_queue_mutex); - vec_extend(&devices_queue, new_devices.data, new_devices.len); - // Signal that there are new devices - pthread_cond_signal(&devices_queue_cond); - pthread_mutex_unlock(&devices_queue_mutex); - } } -// "Execute" a MessageControllerState: set the led color, rumble and flash using the hidraw interface -void apply_controller_state(PhysicalDevice *dev, MessageControllerState *state) { - printf("HID: (%012lx) Controller state: #%02x%02x%02x flash: (%d, %d) rumble: (%d, %d)\n", dev->uniq, - state->led[0], state->led[1], state->led[2], state->flash_on, state->flash_off, - state->small_rumble, state->big_rumble); +// "Execute" a MessageControllerState: set the led color, rumble and flash using the hidraw interface (Dualshock 4 only) +void apply_controller_state(Controller *c, MessageControllerState *state) { + if (c->ctr.ps4_hidraw && c->dev.hidraw < 0) { + printf("HID: Trying to apply controller state on incompatible device (%012lx)\n", c->dev.uniq); + return; + } + + printf("HID: (%012lx) Controller state: #%02x%02x%02x flash: (%d, %d) rumble: (%d, %d)\n", c->dev.uniq, state->led[0], + state->led[1], state->led[2], state->flash_on, state->flash_off, state->small_rumble, state->big_rumble); uint8_t buf[32] = {0x05, 0xff, 0x00, 0x00}; @@ -372,24 +436,24 @@ void apply_controller_state(PhysicalDevice *dev, MessageControllerState *state) buf[9] = state->flash_on; buf[10] = state->flash_off; - write(dev->hidraw, buf, 32); + write(c->dev.hidraw, buf, 32); if (state->flash_on == 0 && state->flash_off == 0) { // May not be necessary - fsync(dev->hidraw); + fsync(c->dev.hidraw); // Send a second time, to reenable the led - write(dev->hidraw, buf, 32); + write(c->dev.hidraw, buf, 32); }; } // Body of the hid thread -void *hid_thread() { +void *hid_thread(void *arg) { printf("HID: start\n"); + config = arg; poll_devices_init(); while (1) { poll_devices(); - - nanosleep(&POLL_DEVICE_INTERVAL, NULL); + nanosleep(&config->poll_interval, NULL); } return NULL; diff --git a/hid.h b/hid.h index a4bcefa..d0d64de 100644 --- a/hid.h +++ b/hid.h @@ -2,6 +2,7 @@ #ifndef HID_H_ #define HID_H_ #include "net.h" +#include "server.h" #include #include @@ -29,9 +30,15 @@ typedef struct { MessageDeviceInfo device_info; } PhysicalDevice; -void *hid_thread(); -void return_device(PhysicalDevice *dev); -PhysicalDevice get_device(); -void apply_controller_state(PhysicalDevice *dev, MessageControllerState *state); +typedef struct { + PhysicalDevice dev; + ServerConfigController ctr; +} Controller; + +void *hid_thread(void *arg); +void return_device(Controller *c); +void forget_device(Controller *c); +Controller *get_device(char *tag); +void apply_controller_state(Controller *c, MessageControllerState *state); #endif diff --git a/json.c b/json.c index 36e6549..b12412b 100644 --- a/json.c +++ b/json.c @@ -22,6 +22,16 @@ JSONError json_errno() { return jerrno; } // Get the location of the last json parsing error size_t json_errloc() { return jerr_index; } +static inline bool is_primitive(const JSONAdapter *adapter) { return adapter->props == NULL; } + +static const char *json_type_name(JSONType type) { + if (type > 0 && type < 7) { + return JSONTypeName[type]; + } else { + return JSONTypeName[0]; + } +} + // Shorthand to set jerno and return -1; // i.e // ```c @@ -111,27 +121,27 @@ static inline int json_parse_string(const char **buf, const char *buf_end, uint8 return set_jerrno(DstOverflow); } - *(*dst)++ = 0b11000000 | (un_codepoint >> 6 & 0b011111); - *(*dst)++ = 0b10000000 | (un_codepoint >> 0 & 0b111111); + *(*dst)++ = 0xC0 | (un_codepoint >> 6 & 0x1F); + *(*dst)++ = 0x80 | (un_codepoint >> 0 & 0x3F); header->len += 2; } else if (un_codepoint <= 0xffff) { // 3 byte codepoint if (*dst + 3 >= dst_end) { return set_jerrno(DstOverflow); } - *(*dst)++ = 0b11100000 | (un_codepoint >> 12 & 0b1111); - *(*dst)++ = 0b10000000 | (un_codepoint >> 6 & 0b111111); - *(*dst)++ = 0b10000000 | (un_codepoint >> 0 & 0b111111); + *(*dst)++ = 0xE0 | (un_codepoint >> 12 & 0x0F); + *(*dst)++ = 0x80 | (un_codepoint >> 6 & 0x3F); + *(*dst)++ = 0x80 | (un_codepoint >> 0 & 0x3F); header->len += 3; } else if (un_codepoint <= 0x10ffff) { // 4 byte codepoint if (*dst + 4 >= dst_end) { return set_jerrno(DstOverflow); } - *(*dst)++ = 0b11110000 | (un_codepoint >> 18 & 0b111); - *(*dst)++ = 0b10000000 | (un_codepoint >> 12 & 0b111111); - *(*dst)++ = 0b10000000 | (un_codepoint >> 6 & 0b111111); - *(*dst)++ = 0b10000000 | (un_codepoint >> 0 & 0b111111); + *(*dst)++ = 0xF0 | (un_codepoint >> 18 & 0x07); + *(*dst)++ = 0x80 | (un_codepoint >> 12 & 0x3F); + *(*dst)++ = 0x80 | (un_codepoint >> 6 & 0x3F); + *(*dst)++ = 0x80 | (un_codepoint >> 0 & 0x3F); header->len += 4; } else { // Illegal codepoint return set_jerrno(StringBadUnicode); @@ -431,6 +441,7 @@ static int json_parse_array(const char **buf, const char *buf_end, uint8_t **res } if (**buf == ']') { // Array is empty + (*buf)++; header->len = 0; return 0; } @@ -490,6 +501,7 @@ static int json_parse_object(const char **buf, const char *buf_end, uint8_t **re return set_jerrno(SrcOverflow); } if (**buf == '}') { + (*buf)++; // The object is empty header->len = 0; return 0; @@ -661,49 +673,186 @@ void json_print_value_priv(uint8_t **buf) { // /!\ doesn't handle strings well void json_print_value(uint8_t *buf) { json_print_value_priv(&buf); } -// Loop over adapters and set accordingly -static void json_adapt_set(uint8_t *buf, JSONAdapter *adapters, size_t adapter_count, void *ptr, char *path) { - JSONHeader *header = (JSONHeader *)buf; +void json_print_buffer(uint8_t *buf) { + uint8_t *end = buf + align_8(((JSONHeader *)buf)->len) + sizeof(JSONHeader); + while (buf < end) { + JSONHeader *header = (JSONHeader *)buf; + printf("[\033[32m%s\033[0m][\033[31m%lu\033[0m]", json_type_name(header->type), align_8(header->len)); + buf += sizeof(JSONHeader); - for (int i = 0; i < adapter_count; i++) { - if (strcmp(path, adapters[i].path) == 0 && header->type == adapters[i].type) { + if (header->type == Object || header->type == Array) + continue; - void *p = ptr + adapters[i].offset; + printf("[\033[34m"); + switch (header->type) { + case Number: + printf("%lf", *(double *)buf); + break; + case Boolean: + printf("%s", *(uint64_t *)buf == 1 ? "true" : "false"); + break; + case Null: + printf("null"); + break; + case String: + printf("\"%.*s\"", header->len, (char *)buf); + break; + } + printf("\033[0m]"); - switch (header->type) { - case String: { - char *v = malloc(header->len + 1); - strncpy(v, (char *)(buf + sizeof(JSONHeader)), header->len); - v[header->len] = '\0'; - *(char **)p = v; - } break; - case Number: - *(double *)p = *(double *)(buf + sizeof(JSONHeader)); - break; - case Boolean: - *(bool *)p = *(uint64_t *)(buf + sizeof(JSONHeader)) == 1; - break; + buf += align_8(header->len); + } + printf("\n"); +} + +static inline bool ends_with(const char *str, const char *pat) { + size_t strl = strlen(str); + size_t patl = strlen(pat); + return strl >= patl && strcmp(str + strl - patl, pat) == 0; +} + +static void json_adapt_set_defaults(const JSONAdapter *adapter, void *ptr) { + if (!is_primitive(adapter)) { + for (int i = 0; i < adapter->prop_count; i++) { + uint8_t *p = (uint8_t*)ptr + adapter->props[i].offset; + + if (ends_with(adapter->props[i].path, "[]")) { + *(size_t *)(p + sizeof(void *)) = 0; + } + + if (adapter->props[i].default_func != NULL) { + adapter->props[i].default_func(p); + } else if (!is_primitive(adapter->props[i].type)) { + json_adapt_set_defaults(adapter->props[i].type, p); } } } } // Run adapters on a value -static void json_adapt_priv(uint8_t **buf, JSONAdapter *adapters, size_t adapter_count, void *ptr, +// buf: is a reference of the pointer to the json buffer +// adapter: is the adapter to run +// ptr: points where to write the data +// path_buffer: points to the begining of the path buffer +// full_path: points to the "current" path +// path: points to the end of the current path (most of the times) +static void json_adapt_priv(uint8_t **buf, const JSONAdapter *adapter, void *ptr, char *path_buffer, char *full_path, char *path) { JSONHeader *header = (JSONHeader *)*buf; + if (is_primitive(adapter)) { + // The type of a primitive adapter is stored in prop_count + JSONType type = adapter->prop_count; + + if (type != header->type) { + printf("JSON: Mismatched type on %s: expected %s got %s\n", path_buffer, json_type_name(type), + json_type_name(header->type)); + return; + } + + *buf += sizeof(JSONHeader); + + if (type == Boolean) { + + *(bool *)ptr = *(uint64_t *)(*buf) == 1; + + } else if (type == Number) { + + *(double *)ptr = *(double *)(*buf); + + } else if (type == String) { + + char *v = malloc(header->len + 1); + strncpy(v, (char *)(*buf), header->len); + v[header->len] = '\0'; + *(char **)ptr = v; + + } else { + printf("JSON: Unknown or illegal primitive adapter of type %s\n", json_type_name(type)); + } + + return; + } + + // This is only true once, so we set default values there + if (path == full_path) { + json_adapt_set_defaults(adapter, ptr); + } + + if (header->type == Array) { + path[0] = '['; + path[1] = ']'; + path[2] = '\0'; + } + + uint8_t buffer_small[64]; + + for (int i = 0; i < adapter->prop_count; i++) { + if (strcmp(adapter->props[i].path, full_path) == 0) { + uint8_t *p = (uint8_t*)ptr + adapter->props[i].offset; + size_t size = adapter->props[i].type->size; + + if (header->type == Array) { + uint8_t *array_buf = *buf + sizeof(JSONHeader); + uint8_t *end = array_buf + header->len; + size_t len; + for (len = 0; array_buf < end; len++) { + array_buf += align_8(((JSONHeader *)array_buf)->len); + array_buf += sizeof(JSONHeader); + }; + + uint8_t *array_ptr = malloc(len * size); + + array_buf = *buf + sizeof(JSONHeader); + for (size_t index = 0; index < len; index++) { + path[0] = '.'; + path[1] = '\0'; + json_adapt_priv(&array_buf, adapter->props[i].type, array_ptr + index * size, path_buffer, + path, path); + path[0] = '\0'; + } + + if (adapter->props[i].transformer != NULL) { + printf("JSON: Transformers aren't yet allowed on arrays\n"); + } + + *(void **)p = array_ptr; + *(size_t *)(p + sizeof(void *)) = len; + } else { + void *tmp_ptr; + if (size <= 64) { + tmp_ptr = buffer_small; + } else { + tmp_ptr = malloc(size); + } + + uint8_t *new_buf = *buf; + path[0] = '.'; + path[1] = '\0'; + json_adapt_priv(&new_buf, adapter->props[i].type, tmp_ptr, path_buffer, path, path); + path[0] = '\0'; + + if (adapter->props[i].transformer != NULL) { + adapter->props[i].transformer(tmp_ptr, p); + } else { + memcpy(p, tmp_ptr, size); + } + + if (tmp_ptr != buffer_small) { + free(tmp_ptr); + } + } + } + } + switch (header->type) { case String: - json_adapt_set(*buf, adapters, adapter_count, ptr, full_path); *buf += sizeof(JSONHeader) + align_8(header->len); break; case Number: - json_adapt_set(*buf, adapters, adapter_count, ptr, full_path); *buf += sizeof(JSONHeader) + sizeof(double); break; case Boolean: - json_adapt_set(*buf, adapters, adapter_count, ptr, full_path); *buf += sizeof(JSONHeader) + 8; break; case Null: @@ -714,7 +863,7 @@ static void json_adapt_priv(uint8_t **buf, JSONAdapter *adapters, size_t adapter uint8_t *end = *buf + header->len; for (size_t index = 0; *buf < end; index++) { int len = sprintf(path, ".%lu", index); - json_adapt_priv(buf, adapters, adapter_count, ptr, full_path, path + len); + json_adapt_priv(buf, adapter, ptr, path_buffer, full_path, path + len); } } break; case Object: { @@ -727,14 +876,14 @@ static void json_adapt_priv(uint8_t **buf, JSONAdapter *adapters, size_t adapter int len = sprintf(path, ".%.*s", key_header->len, *buf); *buf += align_8(key_header->len); - json_adapt_priv(buf, adapters, adapter_count, ptr, full_path, path + len); + json_adapt_priv(buf, adapter, ptr, path_buffer, full_path, path + len); } } break; } } -// Run adapters on a json value -void json_adapt(uint8_t *buf, JSONAdapter *adapters, size_t adapter_count, void *ptr) { +// Run adapter on a json value +void json_adapt(uint8_t *buf, const JSONAdapter *adapter, void *ptr) { char path[512] = "."; - json_adapt_priv(&buf, adapters, adapter_count, ptr, path, path); + json_adapt_priv(&buf, adapter, ptr, path, path, path); } diff --git a/json.h b/json.h index 5249814..916741f 100644 --- a/json.h +++ b/json.h @@ -4,6 +4,7 @@ #include #include #include +#include typedef struct __attribute__((packed, aligned(8))) { uint32_t type; @@ -37,6 +38,36 @@ typedef enum { JERRORNO_MAX = 10 } JSONError; +struct JSONAdapter; + +typedef struct { + char *path; + const struct JSONAdapter *type; + size_t offset; + // Function setting default value + void (*default_func)(void *ptr); + // Optional transformer, can be NULL + void (*transformer)(void *arg, void *ptr); +} JSONPropertyAdapter; + +struct JSONAdapter { + const JSONPropertyAdapter *props; + size_t size; + size_t prop_count; +}; +typedef struct JSONAdapter JSONAdapter; + +void json_adapt(uint8_t *buf, const JSONAdapter *adapter, void *ptr); +int json_parse(const char *src, size_t src_len, uint8_t *dst, size_t dst_len); +void json_print_value(uint8_t *buf); +const char *json_strerr(); +size_t json_errloc(); +JSONError json_errno(); + +extern const JSONAdapter NumberAdapter; +extern const JSONAdapter StringAdapter; +extern const JSONAdapter BooleanAdapter; + #ifdef JSON_C_ static const char *JSONErrorMessage[JERRORNO_MAX + 1] = { "No error", @@ -51,20 +82,32 @@ static const char *JSONErrorMessage[JERRORNO_MAX + 1] = { "Unexpected character in object", "?", }; + +static const char * JSONTypeName[7] = { + "[Unknown]", + "String", + "Number", + "Object", + "Array", + "Boolean", + "Null", +}; + +const JSONAdapter NumberAdapter = { + .prop_count = Number, + .props = NULL, + .size = sizeof(double), +}; +const JSONAdapter StringAdapter = { + .prop_count = String, + .props = NULL, + .size = sizeof(char *), +}; +const JSONAdapter BooleanAdapter = { + .prop_count = Boolean, + .props = NULL, + .size = sizeof(bool), +}; #endif -// See client.c for usage of adapters -typedef struct { - char *path; - JSONType type; - size_t offset; -} JSONAdapter; - -void json_adapt(uint8_t *buf, JSONAdapter *adapters, size_t adapter_count, void *ptr); -int json_parse(const char *src, size_t src_len, uint8_t *dst, size_t dst_len); -void json_print_value(uint8_t *buf); -const char *json_strerr(); -size_t json_errloc(); -JSONError json_errno(); - #endif diff --git a/main.c b/main.c index e622de0..eeb9c6b 100644 --- a/main.c +++ b/main.c @@ -3,30 +3,26 @@ #include "server.h" #include "util.h" -#include #include #include #include const char *USAGE[] = { - "jsfw client [address] [port]\n", - "jsfw server [port]\n", + "jsfw client [address] [port] [config]\n", + "jsfw server [port] [config]\n", }; // Start the server -void server(uint16_t port) { - printf("[Server (0.0.0.0:%u)]\n\n", port); +void server(uint16_t port, char *config_path) { + printf("[Server (0.0.0.0:%u)] <- %s\n\n", port, config_path); - pthread_t thread; - pthread_create(&thread, NULL, hid_thread, NULL); - - server_run(port); + server_run(port, config_path); } // Start the client -void client(char *address, uint16_t port) { - printf("[Client (%s:%d)]\n\n", address, port); - client_run(address, port); +void client(char *address, uint16_t port, char *config_path) { + printf("[Client (%s:%d)] <- %s\n\n", address, port, config_path); + client_run(address, port, config_path); } int main(int argc, char *argv[]) { @@ -39,21 +35,23 @@ int main(int argc, char *argv[]) { char *mode = argv[1]; if (strcmp(mode, "server") == 0) { - if (argc < 3) { + if (argc < 4) { panicf("Usage: %s", USAGE[1]); } - uint16_t port = parse_port(argv[2]); - server(port); + uint16_t port = parse_port(argv[2]); + char *config_path = argv[3]; + server(port, config_path); } else if (strcmp(mode, "client") == 0) { - if (argc < 4) { + if (argc < 5) { panicf("Usage: %s", USAGE[0]); } - char *address = argv[2]; - uint16_t port = parse_port(argv[3]); - client(address, port); + char *address = argv[2]; + uint16_t port = parse_port(argv[3]); + char *config_path = argv[4]; + client(address, port, config_path); } else { printf("Unknown mode: '%s'\n", mode); diff --git a/net.c b/net.c index a1c3edd..6da8fc5 100644 --- a/net.c +++ b/net.c @@ -1,6 +1,9 @@ #include "net.h" +#include "util.h" + #include +#include // Deserialize the message in buf, buf must be at least 4 aligned. Returns -1 on error, otherwise returns 0 // and writes result to dst @@ -12,7 +15,7 @@ int msg_deserialize(const uint8_t *buf, size_t len, Message *restrict dst) { uint8_t code_byte = buf[0]; MessageCode code = (MessageCode)code_byte; - uint16_t abs, rel, key, *buf16; + uint16_t abs, rel, key, index, *buf16; switch (code) { case DeviceInfo: @@ -20,14 +23,16 @@ int msg_deserialize(const uint8_t *buf, size_t len, Message *restrict dst) { return -1; // buf + 2: a byte for code and a byte for padding buf16 = (uint16_t *)(buf + 2); - abs = buf16[0]; - rel = buf16[1]; - key = buf16[2]; - buf += 8; + index = buf16[0]; + abs = buf16[1]; + rel = buf16[2]; + key = buf16[3]; + buf += 12; if (MSS_DEVICE_INFO(abs, rel, key) > len) return -1; dst->device_info.code = code; + dst->device_info.index = index; dst->device_info.abs_count = abs; dst->device_info.rel_count = rel; dst->device_info.key_count = key; @@ -64,14 +69,16 @@ int msg_deserialize(const uint8_t *buf, size_t len, Message *restrict dst) { // buf + 2: a byte for code and a byte of padding buf16 = (uint16_t *)(buf + 2); - abs = buf16[0]; - rel = buf16[1]; - key = buf16[2]; - buf += 8; + index = buf16[0]; + abs = buf16[1]; + rel = buf16[2]; + key = buf16[3]; + buf += 12; if (len < MSS_DEVICE_REPORT(abs, rel, key)) return -1; dst->device_report.code = code; + dst->device_report.index = index; dst->device_report.abs_count = abs; dst->device_report.rel_count = rel; dst->device_report.key_count = key; @@ -95,13 +102,59 @@ int msg_deserialize(const uint8_t *buf, size_t len, Message *restrict dst) { return -1; dst->code = code; - dst->controller_state.led[0] = buf[1]; - dst->controller_state.led[1] = buf[2]; - dst->controller_state.led[2] = buf[3]; - dst->controller_state.small_rumble = buf[4]; - dst->controller_state.big_rumble = buf[5]; - dst->controller_state.flash_on = buf[6]; - dst->controller_state.flash_off = buf[7]; + dst->controller_state.index = *(uint16_t *)(buf + 2); + dst->controller_state.led[0] = buf[4]; + dst->controller_state.led[1] = buf[5]; + dst->controller_state.led[2] = buf[6]; + dst->controller_state.small_rumble = buf[7]; + dst->controller_state.big_rumble = buf[8]; + dst->controller_state.flash_on = buf[9]; + dst->controller_state.flash_off = buf[10]; + return 0; + case Request: { + if (len < 3) + return -1; + + dst->code = code; + dst->request.request_count = *(uint16_t *)(buf + 2); + buf += 4; // 1 bytes for code, 1 byte for padding and 2 bytes for count + + int count = dst->request.request_count; + char **tags = malloc(count * sizeof(char *)); + // The length of the message, will be updated as we read more. + int expected_len = 3; + + for (int i = 0; i < dst->request.request_count; i++) { + expected_len += 2; + if (len < expected_len) { + return -1; + } + + uint16_t str_len = *(uint16_t *)buf; + buf += 2; + + expected_len += align_2(str_len); + if (len < expected_len) { + return -1; + } + + char *str = malloc(str_len + 1); + str[len] = '\0'; + + strncpy(str, (char *)buf, str_len); + + buf += align_2(str_len); + } + + dst->request.requests = tags; + return 0; + } + case DeviceDestroy: + if (len < MSS_DESTROY) + return -1; + + dst->code = code; + dst->destroy.index = *(uint16_t *)(buf + 2); return 0; default: return -1; @@ -129,10 +182,11 @@ int msg_serialize(uint8_t *restrict buf, size_t len, const Message *msg) { // buf + 2: a byte for code and a byte for padding buf16 = (uint16_t *)(buf + 2); // 2 aligned here - buf16[0] = abs; - buf16[1] = rel; - buf16[2] = key; - buf += 8; + buf16[0] = msg->device_info.index; + buf16[1] = abs; + buf16[2] = rel; + buf16[3] = key; + buf += 12; // Back to 4 aligned for (int i = 0; i < abs; i++) { @@ -171,10 +225,11 @@ int msg_serialize(uint8_t *restrict buf, size_t len, const Message *msg) { buf[0] = (uint8_t)msg->code; // buf + 2: a byte for code and a byte for padding buf16 = (uint16_t *)(buf + 2); - buf16[0] = abs; - buf16[1] = rel; - buf16[2] = key; - buf += 8; + buf16[0] = msg->device_report.index; + buf16[1] = abs; + buf16[2] = rel; + buf16[3] = key; + buf += 12; // We're 4 aligned already for (int i = 0; i < abs; i++) { *(uint32_t *)buf = msg->device_report.abs[i]; @@ -195,16 +250,67 @@ int msg_serialize(uint8_t *restrict buf, size_t len, const Message *msg) { return -1; buf[0] = (uint8_t)msg->code; - buf[1] = msg->controller_state.led[0]; - buf[2] = msg->controller_state.led[1]; - buf[3] = msg->controller_state.led[2]; - buf[4] = msg->controller_state.small_rumble; - buf[5] = msg->controller_state.big_rumble; - buf[6] = msg->controller_state.flash_on; - buf[7] = msg->controller_state.flash_off; + + *(uint16_t *)(buf + 2) = msg->controller_state.index; + + buf[4] = msg->controller_state.led[0]; + buf[5] = msg->controller_state.led[1]; + buf[6] = msg->controller_state.led[2]; + buf[7] = msg->controller_state.small_rumble; + buf[8] = msg->controller_state.big_rumble; + buf[9] = msg->controller_state.flash_on; + buf[10] = msg->controller_state.flash_off; return MSS_CONTROLLER_STATE + 1; + case Request: { + int expected_len = MSS_REQUEST(msg->request.request_count); + if (len < expected_len) + return -1; + + buf[0] = (uint8_t)msg->code; + buf16 = (uint16_t *)(buf + 2); + buf16[0] = msg->request.request_count; + + buf += 4; + buf16++; + + for (int i = 0; i < msg->request.request_count; i++) { + int str_len = strlen(msg->request.requests[i]); + int byte_len = align_2(str_len); + *buf16++ = str_len; + buf = (uint8_t *)buf16; + + expected_len += byte_len; + if (len < expected_len) { + return -1; + } + + strncpy((char *)buf, msg->request.requests[i], str_len); + buf += byte_len; + // Buf has to be aligned here since byte_len is two aligned and we started off two aligned + buf16 = (uint16_t *)buf; + } + + return expected_len; + } + case DeviceDestroy: + if (len < MSS_DESTROY) + return -1; + + buf[0] = (uint8_t)msg->code; + + *(uint16_t *)(buf + 2) = msg->controller_state.index; + return MSS_DESTROY + 1; default: printf("ERR(msg_serialize): Trying to serialize unknown message of code %d\n", msg->code); return -1; } } + +void msg_free(Message *msg) { + if (msg->code == Request) { + for (int i = 0; i < msg->request.request_count; i++) { + free(msg->request.requests[i]); + } + free(msg->request.requests); + } +} diff --git a/net.h b/net.h index 6d22c07..1ed918a 100644 --- a/net.h +++ b/net.h @@ -6,10 +6,12 @@ #include typedef enum { + NoMessage = 0, DeviceInfo = 1, DeviceReport = 2, DeviceDestroy = 3, ControllerState = 4, + Request = 5, } MessageCode; // Alignment 4 @@ -17,6 +19,8 @@ typedef struct { MessageCode code; // + 1 byte of padding + uint16_t index; + uint16_t abs_count; uint16_t rel_count; uint16_t key_count; @@ -33,7 +37,7 @@ typedef struct { uint16_t key_id[KEY_CNT]; } MessageDeviceInfo; -#define MSS_DEVICE_INFO(abs, rel, key) (8 + abs * 24 + rel * 2 + key * 2 + 1) +#define MSS_DEVICE_INFO(abs, rel, key) (10 + abs * 24 + rel * 2 + key * 2 + 1) // MSS -> Message Serialized Size: // Size of the data of the message when serialized (no alignment / padding) @@ -41,6 +45,7 @@ typedef struct { typedef struct { MessageCode code; // + 1 byte of padding + uint16_t index; uint16_t abs_count; uint16_t rel_count; @@ -50,28 +55,49 @@ typedef struct { uint32_t rel[REL_CNT]; uint8_t key[KEY_CNT]; } MessageDeviceReport; -#define MSS_DEVICE_REPORT(abs, rel, key) (6 + abs * 4 + rel * 4 + key * 1 + 1) +#define MSS_DEVICE_REPORT(abs, rel, key) (8 + abs * 4 + rel * 4 + key * 1 + 1) // 1 aligned typedef struct { MessageCode code; + // + 1 byte of padding - uint8_t led[3]; - uint8_t small_rumble; - uint8_t big_rumble; - uint8_t flash_on; - uint8_t flash_off; + uint16_t index; + uint8_t led[3]; + uint8_t small_rumble; + uint8_t big_rumble; + uint8_t flash_on; + uint8_t flash_off; } MessageControllerState; -#define MSS_CONTROLLER_STATE 7 +#define MSS_CONTROLLER_STATE 10 + +typedef struct { + MessageCode code; + + char **requests; + uint16_t request_count; +} MessageRequest; +#define MSS_REQUEST(count) (2 + 2 * count) + +typedef struct { + MessageCode code; + // + 1 byte of padding + + uint16_t index; +} MessageDestroy; +#define MSS_DESTROY 3 typedef union { MessageCode code; + MessageRequest request; + MessageDestroy destroy; MessageDeviceInfo device_info; MessageDeviceReport device_report; MessageControllerState controller_state; } Message; -int msg_deserialize(const uint8_t *buf, size_t len, Message *restrict dst); -int msg_serialize(uint8_t *restrict buf, size_t len, const Message *msg); +int msg_deserialize(const uint8_t *buf, size_t len, Message *restrict dst); +int msg_serialize(uint8_t *restrict buf, size_t len, const Message *msg); +void msg_free(Message *msg); #endif diff --git a/server.c b/server.c index 59ffcb1..5c7b858 100644 --- a/server.c +++ b/server.c @@ -1,14 +1,20 @@ +#include "server.h" + #include "const.h" #include "hid.h" +#include "json.h" #include "net.h" #include "util.h" +#include "vec.h" #include #include +#include #include #include #include #include +#include #include #include #include @@ -22,6 +28,181 @@ struct Connection { uint32_t id; }; +struct DeviceThreadArgs { + int index; + char *tag; + Controller **controller; + struct Connection *conn; +}; + +static void default_timespec(void *ptr) { *(struct timespec *)ptr = POLL_DEVICE_INTERVAL; } +static void default_request_timeout(void *ptr) { *(uint32_t *)ptr = REQUEST_TIMEOUT; } + +const JSONPropertyAdapter FilterAdapterProps[] = { + {".mac_address", &StringAdapter, offsetof(ControllerFilter, mac_address), default_to_zero_u64, tsf_strmac_to_u64}, + {".vendor", &StringAdapter, offsetof(ControllerFilter, vendor), default_to_negative_one_i32, tsf_hex_to_i32 }, + {".product", &StringAdapter, offsetof(ControllerFilter, product), default_to_negative_one_i32, tsf_hex_to_i32 }, + {".js", &BooleanAdapter, offsetof(ControllerFilter, js), default_to_false, NULL }, +}; +const JSONAdapter FilterAdapter = { + .props = FilterAdapterProps, + .prop_count = sizeof(FilterAdapterProps) / sizeof(JSONPropertyAdapter), + .size = sizeof(ControllerFilter), +}; + +const JSONPropertyAdapter ControllerAdapterProps[] = { + {".filter", &FilterAdapter, offsetof(ServerConfigController, filter), NULL, NULL}, + {".tag", &StringAdapter, offsetof(ServerConfigController, tag), default_to_null, NULL}, + {".properties.duplicate", &BooleanAdapter, offsetof(ServerConfigController, duplicate), default_to_false, NULL}, + {".properties.ps4_hidraw", &BooleanAdapter, offsetof(ServerConfigController, ps4_hidraw), default_to_false, NULL}, +}; +const JSONAdapter ControllerAdapter = { + .props = ControllerAdapterProps, + .prop_count = sizeof(ControllerAdapterProps) / sizeof(JSONPropertyAdapter), + .size = sizeof(ServerConfigController), +}; + +const JSONPropertyAdapter ConfigAdapterProps[] = { + {".controllers[]", &ControllerAdapter, offsetof(ServerConfig, controllers), default_to_null, NULL }, + {".poll_interval", &NumberAdapter, offsetof(ServerConfig, poll_interval), default_timespec, tsf_numsec_to_timespec}, + {".request_timeout", &NumberAdapter, offsetof(ServerConfig, request_timeout), default_request_timeout, tsf_numsec_to_intms } +}; +const JSONAdapter ConfigAdapter = { + .props = ConfigAdapterProps, + .prop_count = sizeof(ConfigAdapterProps) / sizeof(JSONPropertyAdapter), + .size = sizeof(ServerConfig), +}; + +static ServerConfig config; +static pthread_key_t device_args_key; +static sigset_t empty_sigset; + +#define TRAP(sig, handler) \ + if (sigaction(sig, &(struct sigaction){.sa_handler = handler, .sa_mask = empty_sigset, .sa_flags = 0}, NULL) != 0) \ + printf("SERVER: can't trap " #sig ".\n") + +void device_thread_exit() { + struct DeviceThreadArgs *args = pthread_getspecific(device_args_key); + + Controller * ctr = *args->controller; + if(ctr != NULL) { + return_device(ctr); + } + + free(args->tag); + free(args); + pthread_exit(NULL); +} + +void *device_thread(void *args_) { + struct DeviceThreadArgs *args = args_; + + pthread_setspecific(device_args_key, args); + + TRAP(SIGTERM, device_thread_exit); + + uint8_t buf[2048] __attribute__((aligned(4))) = {0}; + MessageDeviceInfo dev_info; + + while (true) { + *args->controller = NULL; + Controller *ctr = get_device(args->tag); + *args->controller = ctr; + dev_info = ctr->dev.device_info; + dev_info.index = args->index; + + printf("CONN(%d): [%d] Found suitable [%s] device: '%s' (%012lx)\n", args->conn->id, args->index, args->tag, + ctr->dev.name, ctr->dev.uniq); + + // Send over device info + { + int len = msg_serialize(buf, 2048, (Message *)&dev_info); + if (write(args->conn->socket, buf, len) == -1) { + printf("CONN(%d): [%d] Couldn't send device info\n", args->conn->id, args->index); + break; + } + } + + MessageDeviceReport report = {0}; + + report.code = DeviceReport; + report.abs_count = ctr->dev.device_info.abs_count; + report.rel_count = ctr->dev.device_info.rel_count; + report.key_count = ctr->dev.device_info.key_count; + + while (true) { + struct input_event event; + + int len = read(ctr->dev.event, &event, sizeof(struct input_event)); + + if (len <= 0) { + // We lost the device, so we mark it as broken (we forget it) and try to get a new one (in the next iteration of + // the outer while) + forget_device(ctr); + break; + } + + if (len < sizeof(struct input_event)) { + printf("CONN(%d): [%d] error reading event\n", args->conn->id, args->index); + continue; + } + + if (event.type == EV_SYN) { + int len = msg_serialize(buf, 2048, (Message *)&report); + + if (len < 0) { + printf("CONN(%d): [%d] Couldn't serialize report %d\n", args->conn->id, args->index, len); + continue; + }; + + send(args->conn->socket, buf, len, 0); + } else if (event.type == EV_ABS) { + int index = ctr->dev.mapping.abs_indices[event.code]; + + if (index < 0) { + printf("CONN(%d): [%d] Invalid abs\n", args->conn->id, args->index); + continue; + }; + + report.abs[index] = event.value; + } else if (event.type == EV_REL) { + int index = ctr->dev.mapping.rel_indices[event.code]; + + if (index < 0) { + printf("CONN(%d): [%d] Invalid rel\n", args->conn->id, args->index); + continue; + }; + + report.rel[index] = event.value; + } else if (event.type == EV_KEY) { + int index = ctr->dev.mapping.key_indices[event.code]; + + if (index < 0) { + printf("CONN(%d): [%d] Invalid key\n", args->conn->id, args->index); + continue; + }; + report.key[index] = !!event.value; + } + } + + // Send device destroy message + { + MessageDestroy dstr; + dstr.code = DeviceDestroy; + dstr.index = args->index; + + int len = msg_serialize(buf, 2048, (Message *)&dstr); + if (write(args->conn->socket, buf, len) == -1) { + printf("CONN(%d): [%d] Couldn't send device destroy message\n", args->conn->id, args->index); + break; + } + } + } + + device_thread_exit(); + return NULL; +} + void *server_handle_conn(void *args_) { struct Connection *args = args_; @@ -36,154 +217,175 @@ void *server_handle_conn(void *args_) { if (setsockopt(args->socket, SOL_TCP, TCP_KEEPINTVL, &TCP_KEEPALIVE_RETRY_INTERVAL, sizeof(int)) != 0) printf("ERR(server_handle_conn): Setting idle retry interval\n"); - uint8_t buf[2048] __attribute__((aligned(4))) = {}; + uint8_t buf[2048] __attribute__((aligned(4))) = {0}; - PhysicalDevice dev = get_device(); - printf("CONN(%u): got device '%s'\n", args->id, dev.name); + char *closing_message = ""; + bool got_request = false; + Vec device_threads = vec_of(pthread_t); + Vec device_controllers = vec_of(Controller *); - char *closing_message = ""; - - int len = msg_serialize(buf, 2048, (Message *)&dev.device_info); - if (len > 0) { - if (write(args->socket, buf, len) == -1) { - perror("SERVER: Couldn't send device info, "); - closing_message = "Socket error"; - goto conn_end; - } - } else { - perror("SERVER: Couldn't serialize device info, "); - closing_message = "Device info error"; - goto conn_end; - } - - struct pollfd pfds[2] = {}; - struct pollfd *socket_poll = &pfds[0]; - struct pollfd *event_poll = &pfds[1]; - - socket_poll->fd = args->socket; - socket_poll->events = POLLIN; - event_poll->fd = dev.event; - event_poll->events = POLLIN; - - MessageDeviceReport report = {}; - - report.code = DeviceReport; - report.abs_count = dev.device_info.abs_count; - report.rel_count = dev.device_info.rel_count; - report.key_count = dev.device_info.key_count; + struct pollfd pfd = {.fd = args->socket, .events = POLLIN}; while (1) { - int rc = poll(pfds, 2, -1); - if (rc < 0) { // error (connection closed) + int rc = poll(&pfd, 1, config.request_timeout); + + // If poll timed out + if (rc == 0) { + if (!got_request) { + printf("CONN(%d): Didn't get a device request within %i.%03ds\n", args->id, config.request_timeout / 1000, + config.request_timeout % 1000); + closing_message = "Timed out"; + goto conn_end; + } + + continue; + } else if (rc < 0) { // If poll failed (basically never) closing_message = "Poll error"; goto conn_end; } - // Shutdown connection if we lost the peer - if (socket_poll->revents & POLLHUP || socket_poll->revents & POLLERR) { + // Test for error on socket + if (pfd.revents & POLLHUP || pfd.revents & POLLERR) { closing_message = "Lost peer"; goto conn_end; } - if (socket_poll->revents & POLLIN) { - int len = recv(args->socket, buf, 2048, 0); - - if (len <= 0) { - closing_message = "Lost peer"; - goto conn_end; - } - - Message msg; - if (msg_deserialize(buf, len, &msg) == 0) { - - if (msg.code == ControllerState) { - apply_controller_state(&dev, (MessageControllerState *)&msg); - } else { - printf("CONN(%d): Illegal message\n", args->id); - } - - } else { - printf("CONN(%d): Couldn't parse message.\n", args->id); - } - } - - // Shutdown connection if we lost the device - if (event_poll->revents & POLLHUP || event_poll->revents & POLLERR) { - closing_message = "Lost device"; + // Receive data + int len = recv(args->socket, buf, 2048, 0); + if (len <= 0) { + closing_message = "Lost peer (from recv)"; goto conn_end; } - if (event_poll->revents & POLLIN) { - struct input_event event; + // Parse message + Message msg; + if (msg_deserialize(buf, len, &msg) != 0) { + printf("CONN(%d): Couldn't parse message.\n", args->id); + continue; + } - int len = read(dev.event, &event, sizeof(struct input_event)); - - if (len <= 0) { - closing_message = "Lost device"; - goto conn_end; - } - - if (len < sizeof(struct input_event)) { - printf("CONN(%d): error reading event\n", args->id); + // Handle message + if (msg.code == ControllerState) { + int i = msg.controller_state.index; + if (i >= device_controllers.len) { + printf("CONN(%d): Invalid controller index in controller state message\n", args->id); continue; } - if (event.type == EV_SYN) { - int len = msg_serialize(buf, 2048, (Message *)&report); - - if (len < 0) { - printf("CONN(%d): Couldn't serialize report %d\n", args->id, len); - continue; - }; - - write(args->socket, buf, len); - } else if (event.type == EV_ABS) { - int index = dev.mapping.abs_indices[event.code]; - - if (index < 0) { - printf("CONN(%d): Invalid abs\n", args->id); - continue; - }; - - report.abs[index] = event.value; - } else if (event.type == EV_REL) { - int index = dev.mapping.rel_indices[event.code]; - - if (index < 0) { - printf("CONN(%d): Invalid rel\n", args->id); - continue; - }; - - report.rel[index] = event.value; - } else if (event.type == EV_KEY) { - int index = dev.mapping.key_indices[event.code]; - - if (index < 0) { - printf("CONN(%d): Invalid key\n", args->id); - continue; - }; - report.key[index] = !!event.value; + Controller *ctr = *(Controller **)vec_get(&device_controllers, i); + if (ctr == NULL) { + printf("CONN(%d): Received controller state message but the device hasn't yet been received\n", args->id); + continue; } + + apply_controller_state(ctr, &msg.controller_state); + } else if (msg.code == Request) { + if (got_request) { + printf("CONN(%d): Illegal Request message after initial request\n", args->id); + msg_free(&msg); + continue; + } + + got_request = true; + + printf("CONN(%d): Got client request\n", args->id); + + for (int i = 0; i < msg.request.request_count; i++) { + int index = device_controllers.len; + Controller *ctr = NULL; + vec_push(&device_controllers, &ctr); + + struct DeviceThreadArgs *dev_args = malloc(sizeof(struct DeviceThreadArgs)); + dev_args->controller = vec_get(&device_controllers, index); + dev_args->tag = strdup(msg.request.requests[i]); + dev_args->conn = args; + dev_args->index = index; + + pthread_t thread; + pthread_create(&thread, NULL, device_thread, dev_args); + vec_push(&device_threads, &thread); + } + + msg_free(&msg); + } else { + printf("CONN(%d): Illegal message\n", args->id); } } conn_end: shutdown(args->socket, SHUT_RDWR); printf("CONN(%u): connection closed (%s)\n", args->id, closing_message); - return_device(&dev); free(args); + for (int i = 0; i < device_threads.len; i++) { + pthread_t thread = *(pthread_t *)vec_get(&device_threads, i); + pthread_kill(thread, SIGTERM); + pthread_join(thread, NULL); + } return NULL; } -void server_run(uint16_t port) { +static int sockfd; + +void clean_exit() { + printf("\rSERVER: exiting\n"); + close(sockfd); + exit(0); +} + +void server_run(uint16_t port, char *config_path) { + sigemptyset(&empty_sigset); + pthread_key_create(&device_args_key, NULL); printf("SERVER: start\n"); + // Parse the config + { + FILE *configfd = fopen(config_path, "r"); + if (configfd == NULL) { + perror("SERVER: Couldn't open config file"); + exit(1); + } + + char *cbuf = malloc(8192); + uint8_t *jbuf = (uint8_t *)cbuf + 4096; + + int len = fread(cbuf, 1, 4096, configfd); + if (json_parse(cbuf, len, jbuf, 4096) != 0) { + printf("SERVER: Couldn't parse config, %s (at index %lu)\n", json_strerr(), json_errloc()); + exit(1); + } + + json_adapt(jbuf, &ConfigAdapter, &config); + + free(cbuf); + fclose(configfd); + } + + // Start the hid thread + { + pthread_t _thread; + pthread_create(&_thread, NULL, hid_thread, &config); + } + + // Start the TCP server + int sock = socket(AF_INET, SOCK_STREAM, 0); if (sock < 0) { panicf("Couldn't open socket\n"); } - struct sockaddr_in addr = {}; + sockfd = sock; + + TRAP(SIGINT, clean_exit); + TRAP(SIGHUP, clean_exit); + TRAP(SIGQUIT, clean_exit); + TRAP(SIGTERM, clean_exit); + + if (setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &(int){1}, sizeof(int)) < 0) + printf("SERVER: error when trying to enable SO_REUSEADDR\n"); + if (setsockopt(sock, SOL_SOCKET, SO_REUSEPORT, &(int){1}, sizeof(int)) < 0) + printf("SERVER: error when trying to enable SO_REUSEPORT\n"); + + struct sockaddr_in addr = {0}; addr.sin_family = AF_INET; addr.sin_addr.s_addr = htonl(INADDR_ANY); addr.sin_port = htons(port); diff --git a/server.h b/server.h index d09e009..a58ff38 100644 --- a/server.h +++ b/server.h @@ -1,8 +1,34 @@ // vi:ft=c #ifndef SERVER_H_ #define SERVER_H_ +#include #include +#include -void server_run(uint16_t port); +typedef struct { + // the 0 mac_address represents no filter + uint64_t mac_address; + // negative values means no filter + int32_t vendor; + // negative values means no filter + int32_t product; + bool js; +} ControllerFilter; + +typedef struct { + ControllerFilter filter; + char *tag; + bool duplicate; + bool ps4_hidraw; +} ServerConfigController; + +typedef struct { + ServerConfigController *controllers; + size_t controller_count; + struct timespec poll_interval; + uint32_t request_timeout; +} ServerConfig; + +void server_run(uint16_t port, char *config_path); #endif diff --git a/server_config.json b/server_config.json new file mode 100644 index 0000000..a363332 --- /dev/null +++ b/server_config.json @@ -0,0 +1,18 @@ +{ + "controllers": [ + { + "filter": { "mac_address": "00:b0:d0:63:c2:26" }, + "tag": "Admin", + "properties": { "duplicate": true } + }, + { + "filter": { "vendor": "054c", "product": "05c4", "js": true }, + "tag": "User" + }, + { + "filter": { "vendor": "046d", "product": "c332"}, + "tag": "Mouse" + } + ], + "poll_interval": 1 +} diff --git a/util.c b/util.c index 7e0ea19..35b23fa 100644 --- a/util.c +++ b/util.c @@ -1,9 +1,11 @@ #include "util.h" +#include #include #include #include #include +#include #ifndef __has_builtin #define __has_builtin(_) 0 @@ -34,3 +36,145 @@ uint8_t parse_hex_digit(char h) { else return 0; } + +// Defaults for json parsing +void default_to_null(void *ptr) { *(void **)ptr = NULL; } +void default_to_false(void *ptr) { *(bool *)ptr = false; } +void default_to_zero_u8(void *ptr) { *(uint8_t *)ptr = 0; } +void default_to_zero_u32(void *ptr) { *(uint32_t *)ptr = 0; } +void default_to_zero_u64(void *ptr) { *(uint64_t *)ptr = 0; } +void default_to_zero_size(void *ptr) { *(size_t *)ptr = 0; } +void default_to_zero_double(void *ptr) { *(double *)ptr = 0.0; } +void default_to_one_size(void *ptr) { *(size_t *)ptr = 1; } +void default_to_negative_one_i32(void *ptr) { *(int32_t *)ptr = -1; } + +// Transformers for json parsing +void tsf_numsec_to_timespec(void *arg, void *ptr) { + double seconds = *(double *)arg; + + struct timespec ts; + ts.tv_sec = floor(seconds); + ts.tv_nsec = (seconds - floor(seconds)) * 1000000000; + + *(struct timespec *)ptr = ts; +} + +void tsf_numsec_to_intms(void *arg, void *ptr) { + double seconds = *(double *)arg; + *(uint32_t *)ptr = seconds * 1000; +} + +void tsf_strmac_to_u64(void *arg, void *ptr) { + char *s = *(char **)arg; + if (strnlen(s, 18) != 17) { + printf("JSON: wrong length for mac address, expected 'xx:xx:xx:xx:xx:xx'\n"); + free(s); + return; + } + uint64_t mac = 0; + for (int i = 0; i < 17; i++) { + char c = s[i]; + uint8_t digit = 0; + + if (c >= '0' && c <= '9') + digit = c - '0'; + else if (c >= 'a' && c <= 'f') + digit = c - 'a' + 10; + else if (c >= 'A' && c <= 'F') + digit = c - 'A' + 10; + else if (c == ':') + continue; + else { + printf("JSON: unexpected character '%c' in mac address at position %i (%s)\n", c, i, s); + free(s); + return; + } + + mac <<= 4; + mac |= digit; + } + free(s); + *(uint64_t *)ptr = mac; +} + +void tsf_hex_to_i32(void *arg, void *ptr) { + char *s = *(char **)arg; + char *f = s; + char c; + int32_t res = 0; + while ((c = *s++) != '\0') { + uint8_t digit = 0; + + if (c >= '0' && c <= '9') + digit = c - '0'; + else if (c >= 'a' && c <= 'f') + digit = c - 'a' + 10; + else if (c >= 'A' && c <= 'F') + digit = c - 'A' + 10; + else { + printf("JSON: unexpected character '%c' in hex string\n", c); + free(f); + return; + } + res <<= 4; + res |= digit; + } + free(f); + *(int32_t *)ptr = res; +} + +void tsf_double_to_size(void *arg, void *ptr) { + double d = *(double *)arg; + *(size_t *)ptr = d; +} + +static uint8_t hex_digit(char c) { + if (c >= '0' && c <= '9') + return c - '0'; + else if (c >= 'a' && c <= 'f') + return c - 'a' + 10; + else if (c >= 'A' && c <= 'F') + return c - 'A' + 10; + else + return 16; +} + +void tsf_hex_to_color(void *arg, void *ptr) { + char *s = *(char **)arg; + int len = strnlen(s, 8); + if (len != 7 || s[0] != '#') { + printf("JSON: bad hex color format expected '#RRGGBB' or '#rrggbb', got '%s'\n", s); + free(s); + return; + } + + uint8_t digit[6] = {0}; + uint8_t *color = ptr; + + for (int i = 1; i < 7; i++) { + digit[i] = hex_digit(s[i]); + if (digit[i] == 16) { + printf("JSON: illegal character in hex color: '%c'\n", s[i]); + free(s); + return; + } + + if (i % 2 == 0) { + uint8_t c = digit[i]; + uint8_t p = digit[i - 1]; + color[i / 2] = (p << 4) | c; + } + } + + free(s); +} + +void tsf_num_to_u8_clamp(void * arg, void *ptr) { + double n = *(double*)arg; + *(uint8_t*)ptr = n > 255.0 ? 255.0 : n < 0.0 ? 0.0 : n; +} + +void tsf_num_to_int(void * arg, void *ptr) { + double n = *(double*)arg; + *(int*)ptr = n; +} diff --git a/util.h b/util.h index a29d86a..11148d0 100644 --- a/util.h +++ b/util.h @@ -13,6 +13,27 @@ uint16_t parse_port(const char *str); static inline bool bit_set(uint8_t *bits, int i) { return bits[i / 8] & (1 << (i % 8)); } // Align n to the next 8 boundary static inline size_t align_8(size_t n) { return (((n - 1) >> 3) + 1) << 3; } +// Align n to the next 8 boundary +static inline size_t align_2(size_t n) { return (((n - 1) >> 1) + 1) << 1; } uint8_t parse_hex_digit(char h); +void default_to_null(void *ptr); +void default_to_false(void *ptr); +void default_to_zero_u8(void *ptr); +void default_to_zero_u32(void *ptr); +void default_to_zero_u64(void *ptr); +void default_to_zero_size(void *ptr); +void default_to_zero_double(void *ptr); +void default_to_one_size(void *ptr); +void default_to_negative_one_i32(void *ptr); + +void tsf_numsec_to_timespec(void *arg, void *ptr); +void tsf_numsec_to_intms(void *arg, void *ptr); +void tsf_strmac_to_u64(void *arg, void *ptr); +void tsf_hex_to_i32(void *arg, void *ptr); +void tsf_double_to_size(void * arg, void * ptr); +void tsf_hex_to_color(void *arg, void *ptr); +void tsf_num_to_u8_clamp(void * arg, void *ptr); +void tsf_num_to_int(void * arg, void *ptr); + #endif diff --git a/vec.c b/vec.c index 200b272..95b6715 100644 --- a/vec.c +++ b/vec.c @@ -40,7 +40,7 @@ static inline void vec_grow(Vec *v, size_t cap) { void vec_push(Vec *v, void *data) { vec_grow(v, v->len + 1); - memcpy(v->data + v->stride * v->len++, data, v->stride); + memcpy((u_int8_t *)v->data + v->stride * v->len++, data, v->stride); } void vec_pop(Vec *v, void *data) { @@ -68,7 +68,7 @@ void vec_insert(Vec *v, void *data, size_t index) { } vec_grow(v, v->len + 1); - void *slot = v->data + index * v->stride; + uint8_t *slot = v->data + index * v->stride; if (index < v->len) { memmove(slot + v->stride, slot, (v->len - index) * v->stride); } @@ -86,7 +86,7 @@ void vec_remove(Vec *v, size_t index, void *data) { return; } - void *slot = v->data + index * v->stride; + uint8_t *slot = v->data + index * v->stride; if (data != NULL) { memcpy(data, slot, v->stride); } @@ -103,7 +103,7 @@ void vec_extend(Vec *v, void *data, size_t len) { return; } vec_grow(v, v->len + len); - memcpy(v->data + v->stride * v->len, data, v->stride * len); + memcpy((uint8_t*)v->data + v->stride * v->len, data, v->stride * len); v->len += len; } diff --git a/vec.h b/vec.h index a023777..62f64a8 100644 --- a/vec.h +++ b/vec.h @@ -2,11 +2,12 @@ #ifndef VEC_H_ #define VEC_H_ #include +#include #define vec_of(type) vec_new(sizeof(type)) typedef struct { - void *data; + uint8_t *data; size_t cap; size_t len; size_t stride;