From afe99fc054d8350737582c84c6bd2c67feee9abf Mon Sep 17 00:00:00 2001 From: viandoxdev Date: Tue, 30 Aug 2022 15:37:34 +0200 Subject: [PATCH] never gonna give you up --- hid.c | 212 +++++++++++++++++++++++++++++++++++-------------------- hid.h | 27 ++++--- main.c | 14 +--- main.h | 5 -- net.c | 196 +++++++++++++++++++++++++++++++++----------------- net.h | 57 +++++++++------ server.c | 58 +++++++++++---- util.c | 26 +++---- util.h | 4 +- 9 files changed, 381 insertions(+), 218 deletions(-) diff --git a/hid.c b/hid.c index b21c219..dbff022 100644 --- a/hid.c +++ b/hid.c @@ -1,19 +1,18 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include "hid.h" -#include "vec.h" #include "main.h" +#include "vec.h" // List of uniq of the currently known devices static Vec devices; @@ -28,34 +27,80 @@ static pthread_cond_t devices_queue_cond = PTHREAD_COND_INITIALIZER; // Mutex for devices static pthread_mutex_t devices_mutex = PTHREAD_MUTEX_INITIALIZER; -static char * DEFAULT_NAME = "Unnamed Device"; +static char *DEFAULT_NAME = "Unnamed Device"; // uniqs are just hexadecimal numbers with colons in between each byte uniq_t parse_uniq(char uniq[17]) { uniq_t res = 0; - for(int i = 0; i < 17; i++) { + for (int i = 0; i < 17; i++) { char c = uniq[i]; - int digit; - 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 continue; + int digit; + 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 + continue; res <<= 4; res += digit; } return res; } -bool filter_event(int fd, char * event) { +static inline bool bit_set(uint8_t *bits, int i) { return bits[i / 8] & (1 << (i % 8)); } + +void setup_device(PhysicalDevice *dev) { + dev->device_info.code = DeviceInfo; + dev->device_info.abs_count = 0; + dev->device_info.rel_count = 0; + dev->device_info.key_count = 0; + + uint8_t bits[EV_MAX] = {}; + uint8_t feat_bits[KEY_MAX] = {}; + ioctl(dev->event, EVIOCGBIT(0, EV_MAX), bits); + for (int i = 0; i < EV_MAX; i++) { + if (bit_set(bits, i)) { + ioctl(dev->event, EVIOCGBIT(i, KEY_MAX), feat_bits); + for (int j = 0; j < KEY_MAX; j++) { + if (bit_set(feat_bits, j)) { + if (i == EV_ABS) { + struct input_absinfo abs; + ioctl(dev->event, EVIOCGABS(j), &abs); + uint8_t index = dev->device_info.abs_count++; + dev->device_info.abs_id[index] = j; + dev->device_info.abs_min[index] = abs.minimum; + dev->device_info.abs_max[index] = abs.maximum; + dev->device_info.abs_fuzz[index] = abs.fuzz; + dev->device_info.abs_flat[index] = abs.flat; + dev->device_info.abs_res[index] = abs.resolution; + dev->mapping.abs_indices[j] = index; + } else if (i == EV_REL) { + uint8_t index = dev->device_info.rel_count++; + dev->device_info.rel_id[index] = j; + dev->mapping.rel_indices[j] = index; + } else if (i == EV_KEY) { + uint8_t index = dev->device_info.key_count++; + dev->device_info.key_id[index] = j; + dev->mapping.key_indices[j] = index; + } + } + } + } + } +} + +bool filter_event(int fd, char *event) { char device_path[64]; snprintf(device_path, 64, "/sys/class/input/%s/device", event); - DIR * device_dir = opendir(device_path); - struct dirent * device_dirent; + DIR *device_dir = opendir(device_path); + struct dirent *device_dirent; bool found = false; - while((device_dirent = readdir(device_dir)) != NULL) { - if(device_dirent->d_type == DT_DIR && strncmp(device_dirent->d_name, "js", 2) == 0) { + while ((device_dirent = readdir(device_dir)) != NULL) { + if (device_dirent->d_type == DT_DIR && strncmp(device_dirent->d_name, "js", 2) == 0) { found = true; break; } @@ -63,45 +108,45 @@ bool filter_event(int fd, char * event) { closedir(device_dir); - if(!found) { + if (!found) { return false; } - + uint16_t info[4]; ioctl(fd, EVIOCGID, info); return info[1] == 0x054c && info[2] == 0x05c4; } void poll_devices_init() { - devices = vec_of(uniq_t); - new_devices = vec_of(PhysicalDevice); + devices = vec_of(uniq_t); + new_devices = vec_of(PhysicalDevice); devices_queue = vec_of(PhysicalDevice); } PhysicalDevice get_device() { pthread_mutex_lock(&devices_queue_mutex); - if(devices_queue.len > 0){ + if (devices_queue.len > 0) { PhysicalDevice r; vec_pop(&devices_queue, &r); pthread_mutex_unlock(&devices_queue_mutex); return r; } - while(devices_queue.len == 0) { + while (devices_queue.len == 0) { pthread_cond_wait(&devices_queue_cond, &devices_queue_mutex); } - + PhysicalDevice res; vec_pop(&devices_queue, &res); - if(devices_queue.len > 0) { + if (devices_queue.len > 0) { pthread_cond_signal(&devices_queue_cond); } pthread_mutex_unlock(&devices_queue_mutex); return res; } -void return_device(PhysicalDevice * dev) { - if(dev->name != NULL && dev->name != DEFAULT_NAME) { +void return_device(PhysicalDevice *dev) { + if (dev->name != NULL && dev->name != DEFAULT_NAME) { printf("HID: Returning device '%s' (%012lx)\n", dev->name, dev->uniq); free(dev->name); } else { @@ -110,9 +155,9 @@ void return_device(PhysicalDevice * dev) { close(dev->event); close(dev->hidraw); pthread_mutex_lock(&devices_mutex); - for(int i = 0; i < devices.len; i++) { - uniq_t * uniq = vec_get(&devices, i); - if(*uniq == dev->uniq) { + for (int i = 0; i < devices.len; i++) { + uniq_t *uniq = vec_get(&devices, i); + if (*uniq == dev->uniq) { vec_remove(&devices, i, NULL); break; } @@ -123,67 +168,61 @@ void return_device(PhysicalDevice * dev) { void poll_devices() { vec_clear(&new_devices); - DIR * input_dir = opendir("/sys/class/input"); - struct dirent * input; - while((input = readdir(input_dir)) != NULL) { + DIR *input_dir = opendir("/sys/class/input"); + struct dirent *input; + while ((input = readdir(input_dir)) != NULL) { // Ignore if the entry isn't a linkg or doesn't start with event - if(input->d_type != DT_LNK || strncmp(input->d_name, "event", 5) != 0) { + if (input->d_type != DT_LNK || strncmp(input->d_name, "event", 5) != 0) { continue; } - PhysicalDevice dev = {}; + PhysicalDevice dev; char event_path[64]; snprintf(event_path, 64, "/dev/input/%s", input->d_name); dev.event = open(event_path, O_RDONLY); - if(dev.event < 0) { + if (dev.event < 0) { continue; } - char name_buf[256] = {}; - char * name; - if(ioctl(dev.event, EVIOCGNAME(256), name_buf) >= 0) + char name_buf[256] = {}; + char *name; + if (ioctl(dev.event, EVIOCGNAME(256), name_buf) >= 0) name = name_buf; else name = DEFAULT_NAME; - if(!filter_event(dev.event, input->d_name)) goto skip; + if (!filter_event(dev.event, input->d_name)) + goto skip; uniq_t uniq; { char uniq_str[17] = {}; - char uniq_path[256]; - snprintf(uniq_path, 256, "/sys/class/input/%s/device/uniq", input->d_name); - - int uniq_fd = open(uniq_path, O_RDONLY); - - if(uniq_fd < 0) goto skip; - - read(uniq_fd, uniq_str, 17); + ioctl(dev.event, EVIOCGUNIQ(17), uniq_str); uniq = parse_uniq(uniq_str); - close(uniq_fd); - // If we couldn't parse the uniq (this assumes uniq can't be zero, which is probably alright) - if(uniq == 0) goto skip; + if (uniq == 0) + goto skip; } 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) { + 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) goto skip; + if (found) + goto skip; dev.uniq = uniq; @@ -193,13 +232,14 @@ void poll_devices() { char hidraw_path[256]; snprintf(hidraw_path, 256, "/sys/class/input/%s/device/device/hidraw", input->d_name); - DIR * hidraw_dir = opendir(hidraw_path); - struct dirent * hidraw = NULL; - while((hidraw = readdir(hidraw_dir)) != NULL) { - if(strncmp(hidraw->d_name, "hidraw", 6) == 0) break; + DIR *hidraw_dir = opendir(hidraw_path); + struct dirent *hidraw = NULL; + while ((hidraw = readdir(hidraw_dir)) != NULL) { + if (strncmp(hidraw->d_name, "hidraw", 6) == 0) + break; } - if(hidraw == NULL) { + if (hidraw == NULL) { printf("Couldn't get hidraw of %s", input->d_name); continue; } @@ -210,14 +250,17 @@ void poll_devices() { } dev.hidraw = open(hidraw_path, O_WRONLY); - if(dev.hidraw < 0) goto skip; + if (dev.hidraw < 0) + goto skip; dev.name = malloc(256); - if(dev.name == NULL) + if (dev.name == NULL) dev.name = DEFAULT_NAME; else strcpy(dev.name, name); + setup_device(&dev); + pthread_mutex_lock(&devices_mutex); vec_push(&devices, &uniq); pthread_mutex_unlock(&devices_mutex); @@ -227,13 +270,13 @@ void poll_devices() { continue; // close open file descriptor and continue -skip: + skip: close(dev.event); continue; }; closedir(input_dir); - if(new_devices.len > 0) { + 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 @@ -242,13 +285,32 @@ skip: } } -void * hid_thread() { +void apply_controller_state(PhysicalDevice *dev, MessageControllerState *state) { + uint8_t buf[32] = {0x05, 0xff, 0x00, 0x00}; + + buf[4] = state->small_rumble; + buf[5] = state->big_rumble; + buf[6] = state->led[0]; + buf[7] = state->led[1]; + buf[8] = state->led[2]; + buf[9] = state->flash_on; + buf[10] = state->flash_off; + + write(dev->hidraw, buf, 32); + if(state->flash_on == 0 && state->flash_off == 0) { + fsync(dev->hidraw); + // Send a second time because it doesn't work otherwise + write(dev->hidraw, buf, 32); + }; +} + +void *hid_thread() { printf("HID: start\n"); poll_devices_init(); - while(1) { + while (1) { poll_devices(); struct timespec ts; - ts.tv_sec = 1; + ts.tv_sec = 1; ts.tv_nsec = 0; nanosleep(&ts, NULL); } diff --git a/hid.h b/hid.h index 7c22d92..847cd27 100644 --- a/hid.h +++ b/hid.h @@ -1,21 +1,32 @@ // vi: set ft=c #ifndef HID_H #define HID_H -#include -#include +#include "net.h" #include "vec.h" +#include +#include +#include typedef uint64_t uniq_t; typedef struct { - int event; - int hidraw; - uniq_t uniq; - char * name; + uint8_t abs_indices[ABS_CNT]; + uint8_t rel_indices[REL_CNT]; + uint8_t key_indices[KEY_CNT]; +} DeviceMap; + +typedef struct { + int event; + int hidraw; + uniq_t uniq; + char *name; + DeviceMap mapping; + MessageDeviceInfo device_info; } PhysicalDevice; -void * hid_thread(); -void return_device(PhysicalDevice * dev); +void *hid_thread(); +void return_device(PhysicalDevice *dev); PhysicalDevice get_device(); +void apply_controller_state(PhysicalDevice *dev, MessageControllerState *state); #endif diff --git a/main.c b/main.c index cdedd82..1841ac4 100644 --- a/main.c +++ b/main.c @@ -12,6 +12,7 @@ #include "main.h" #include "hid.h" #include "server.h" +#include "util.h" const char* USAGE[] = { "jsfw client [address] [port]\n", @@ -19,14 +20,6 @@ const char* USAGE[] = { }; const size_t EVENT_SIZE = sizeof(struct js_event); -void panicf(const char *fmt, ...) { - va_list args; - va_start(args, fmt); - vprintf(fmt, args); - va_end(args); - exit(1); -} - uint16_t parse_port(const char * str) { long long n = atoll(str); if(n <= 0 || n > UINT16_MAX) @@ -37,9 +30,8 @@ uint16_t parse_port(const char * str) { void server(uint16_t port) { printf("Server (port: %u).\n", port); - pthread_t _; - pthread_create(&_, NULL, hid_thread, NULL); - + pthread_t thread; + pthread_create(&thread, NULL, hid_thread, NULL); server_run(port); } diff --git a/main.h b/main.h index 39775d1..31fbbd3 100644 --- a/main.h +++ b/main.h @@ -1,10 +1,5 @@ // vi: set ft=c #ifndef MAIN_H #define MAIN_H -#include -#include -#include "vec.h" - -void panicf(const char * fmt, ...); #endif diff --git a/net.c b/net.c index c45a72e..4e1aa7b 100644 --- a/net.c +++ b/net.c @@ -9,79 +9,145 @@ Message msg_device_info() { return s; } -int msg_deserialize(const uint8_t * buf, size_t len, Message * dst) { +int msg_deserialize(const uint8_t *buf, size_t len, Message *dst) { // Decrement len so that it becomes the len of the data without the code. - if(len-- < 1) return -1; + if (len-- < 1) + return -1; // This ensures that only a byte is read instead of a full enum value - uint8_t code_byte = buf[0]; - MessageCode code = (MessageCode) code_byte; + uint8_t code_byte = buf[0]; + MessageCode code = (MessageCode)code_byte; - switch(code) { - case Heartbeat: - if(MSS_HEARTBEAT > len) return -1; - dst->code = code; - dst->heartbeat.alive = buf[1]; - return 0; - case DeviceInfo: - if(MSS_DEVICE_INFO > len) return -1; - dst->code = code; - return 0; - case DeviceReport: - if(len < MSS_DEVICE_REPORT) return -1; - dst->code = code; - return 0; - case DeviceDestroy: - if(len < MSS_DEVICE_DESTROY) return -1; - dst->code = code; - return 0; - case ControllerState: - if(len < MSS_CONTROLLER_STATE) 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]; - return 0; - default: + switch (code) { + case DeviceInfo: + if (len < 3) return -1; + uint8_t abs = buf[1]; + uint8_t rel = buf[2]; + uint8_t key = buf[3]; + buf += 4; + if (MSS_DEVICE_INFO(abs, rel, key) > len) + return -1; + + dst->code = code; + dst->device_info.abs_count = abs; + dst->device_info.rel_count = rel; + dst->device_info.key_count = key; + + // SOA in c but serialized as AOS + for (int i = 0; i < abs; i++) { + uint32_t *buf32 = (uint32_t *)(buf + 1); + + dst->device_info.abs_id[i] = buf[0]; + dst->device_info.abs_min[i] = buf32[0]; + dst->device_info.abs_max[i] = buf32[1]; + dst->device_info.abs_fuzz[i] = buf32[2]; + dst->device_info.abs_flat[i] = buf32[3]; + dst->device_info.abs_res[i] = buf32[4]; + + buf += 21; + } + + for (int i = 0; i < rel; i++) + dst->device_info.rel_id[i] = *(buf++); + + for (int i = 0; i < key; i++) + dst->device_info.key_id[i] = *(buf++); + + return 0; + case DeviceReport: + if (len < MSS_DEVICE_REPORT) + return -1; + dst->code = code; + return 0; + case DeviceDestroy: + if (len < MSS_DEVICE_DESTROY) + return -1; + dst->code = code; + return 0; + case ControllerState: + if (len < MSS_CONTROLLER_STATE) + 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]; + return 0; + default: + return -1; } } // The indices have to match with msg_deserialize -int msg_serialize(uint8_t * buf, size_t len, Message msg) { - switch(msg.code) { - case Heartbeat: - if(MSS_HEARTBEAT >= len) return -1; - buf[0] = (uint8_t) msg.code; - buf[1] = msg.heartbeat.alive; - return 0; - case DeviceInfo: - if(MSS_DEVICE_INFO >= len) return -1; - buf[0] = (uint8_t) msg.code; - return 0; - case DeviceReport: - if(MSS_DEVICE_REPORT >= len) return -1; - buf[0] = (uint8_t) msg.code; - return 0; - case DeviceDestroy: - if(MSS_DEVICE_DESTROY >= len) return -1; - buf[0] = (uint8_t) msg.code; - return 0; - case ControllerState: - if(MSS_CONTROLLER_STATE >= len) 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; - return 0; - default: +int msg_serialize(uint8_t *buf, size_t len, Message *msg) { + // If len is 0 we can't serialize any message + if (len-- == 0) + return -1; + + switch (msg->code) { + case DeviceInfo:; // semicolon needed here + uint8_t abs = msg->device_info.abs_count; + uint8_t rel = msg->device_info.rel_count; + uint8_t key = msg->device_info.key_count; + if (len < MSS_DEVICE_INFO(abs, rel, len)) return -1; + + buf[0] = (uint8_t)msg->code; + buf[1] = abs; + buf[2] = rel; + buf[3] = key; + buf += 4; + + for (int i = 0; i < abs; i++) { + uint32_t *buf32 = (uint32_t *)(buf + 1); + + buf[0] = msg->device_info.abs_id[i]; + buf32[0] = msg->device_info.abs_min[i]; + buf32[1] = msg->device_info.abs_max[i]; + buf32[2] = msg->device_info.abs_fuzz[i]; + buf32[3] = msg->device_info.abs_flat[i]; + buf32[4] = msg->device_info.abs_res[i]; + + buf += 21; + } + + for (int i = 0; i < rel; i++) + *(buf++) = msg->device_info.rel_id[i]; + + for (int i = 0; i < key; i++) + *(buf++) = msg->device_info.key_id[i]; + + return MSS_DEVICE_INFO(abs, rel, key) + 1; + case DeviceReport: + if (len < MSS_DEVICE_REPORT) + return -1; + + buf[0] = (uint8_t)msg->code; + return MSS_DEVICE_REPORT + 1; + case DeviceDestroy: + if (len < MSS_DEVICE_DESTROY) + return -1; + + buf[0] = (uint8_t)msg->code; + return MSS_DEVICE_DESTROY + 1; + case ControllerState: + if (len < MSS_CONTROLLER_STATE) + 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; + return MSS_CONTROLLER_STATE + 1; + default: + return -1; } } diff --git a/net.h b/net.h index ffb0ee5..133da66 100644 --- a/net.h +++ b/net.h @@ -1,30 +1,39 @@ // vi: set ft=c #ifndef NET_H #define NET_H -#include -#include +#include +#include +#include typedef enum { - Heartbeat = 0, - DeviceInfo = 1, - DeviceReport = 2, - DeviceDestroy = 3, + DeviceInfo = 1, + DeviceReport = 2, + DeviceDestroy = 3, ControllerState = 4, } MessageCode; typedef struct { MessageCode code; - uint8_t alive; -} MessageHeartbeat; -#define MSS_HEARTBEAT 1 + + uint8_t abs_count; + uint8_t rel_count; + uint8_t key_count; + + uint8_t abs_id[ABS_CNT]; + uint32_t abs_min[ABS_CNT]; + uint32_t abs_max[ABS_CNT]; + uint32_t abs_fuzz[ABS_CNT]; + uint32_t abs_flat[ABS_CNT]; + uint32_t abs_res[ABS_CNT]; + + uint8_t rel_id[REL_CNT]; + + uint8_t key_id[KEY_CNT]; +} MessageDeviceInfo; +#define MSS_DEVICE_INFO(abs, rel, key) (3 + abs * 21 + rel * 1 + key * 1) // MSS -> Message Serialized Size: // Size of the data of the message when serialized (no alignment / padding) -typedef struct { - MessageCode code; -} MessageDeviceInfo; -#define MSS_DEVICE_INFO 0 - typedef struct { MessageCode code; } MessageDeviceReport; @@ -35,8 +44,9 @@ typedef struct { } MessageDeviceDestroy; #define MSS_DEVICE_DESTROY 0 -typedef struct { +typedef struct { MessageCode code; + uint8_t led[3]; uint8_t small_rumble; uint8_t big_rumble; @@ -46,15 +56,18 @@ typedef struct { #define MSS_CONTROLLER_STATE 7 typedef union { - MessageCode code; - MessageHeartbeat heartbeat; - MessageDeviceInfo device_info; - MessageDeviceReport device_report; - MessageDeviceDestroy device_destroy; + MessageCode code; + MessageDeviceInfo device_info; + MessageDeviceReport device_report; + MessageDeviceDestroy device_destroy; MessageControllerState controller_state; } Message; -int msg_deserialize(const uint8_t * buf, size_t len, Message * dst); -int msg_serialize(uint8_t * buf, size_t len, Message msg); +// Read a message from a buffer with a length, message goes into dst, returns -1 if a messsage couldn't be +// read +int msg_deserialize(const uint8_t *buf, size_t len, Message *dst); +// Write a message to a buffer with a length, return -1 if the message couldn't be written (buffer not big +// enough). Returns the length of the serialized message if succeeded. +int msg_serialize(uint8_t *buf, size_t len, Message *msg); #endif diff --git a/server.c b/server.c index dca9444..43c5cca 100644 --- a/server.c +++ b/server.c @@ -1,14 +1,19 @@ #include +#include +#include #include #include +#include #include #include #include +#include +#include #include #include "hid.h" -#include "main.h" #include "net.h" +#include "util.h" #include "vec.h" struct Connection { @@ -19,12 +24,13 @@ struct Connection { void *server_handle_conn(void *args_) { struct Connection *args = args_; - printf("THREAD(%u): start\n", args->id); + printf("CONN(%u): start\n", args->id); - int enable = 1; + int enable = true; int idle_time = 10; int keep_count = 5; - int keep_interval = 5; + int keep_interval = 2; + if (setsockopt(args->socket, SOL_SOCKET, SO_KEEPALIVE, &enable, sizeof(enable)) != 0) printf("ERR(server_handle_conn): Enabling socket keepalives on client\n"); if (setsockopt(args->socket, SOL_TCP, TCP_KEEPIDLE, &idle_time, sizeof(idle_time)) != 0) @@ -34,26 +40,50 @@ void *server_handle_conn(void *args_) { if (setsockopt(args->socket, SOL_TCP, TCP_KEEPINTVL, &keep_interval, sizeof(keep_interval)) != 0) printf("ERR(server_handle_conn): Setting idle retry interval\n"); + uint8_t buf[2048]; PhysicalDevice dev = get_device(); - printf("THREAD(%u): got device '%s'\n", args->id, dev.name); + printf("CONN(%u): got device '%s'\n", args->id, dev.name); + + int len = msg_serialize(buf, 2048, (Message *)&dev.device_info); + write(args->socket, buf, len); + + struct pollfd pfd[2] = {}; + pfd[0].fd = args->socket; + pfd[0].events = POLLIN; + pfd[1].fd = dev.event; + pfd[1].events = POLLIN; - uint8_t buf[1024]; while (1) { - int len = recv(args->socket, buf, 1024, MSG_WAITALL); - - if (len <= 0) + int rc = poll(pfd, 1, -1); + if (rc < 0) // error (connection closed) goto conn_end; - Message msg; - if (msg_deserialize(buf, len, &msg) == 0) { + if (pfd[0].revents & POLLIN) { + int len = recv(args->socket, buf, 2048, 0); + if (len <= 0) + 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); + } + } + if (pfd[1].revents & POLLIN) { - } else { - printf("Couldn't parse message.\n"); } } - printf("THREAD(%u): connection closed\n", args->id); conn_end: + shutdown(args->socket, SHUT_RDWR); + printf("CONN(%u): connection closed\n", args->id); return_device(&dev); free(args); return NULL; diff --git a/util.c b/util.c index 2a93b05..ec76976 100644 --- a/util.c +++ b/util.c @@ -1,22 +1,18 @@ +#include +#include +#include +#include + #include "util.h" -#include #ifndef __has_builtin #define __has_builtin(_) 0 #endif -unsigned long log2lu(unsigned long n) { -#if __has_builtin(__builtin_clz) - return sizeof(unsigned long) * CHAR_BIT - __builtin_clz(n) - 1; -#else - unsigned long res = 0; - while(n >>= 1) ++res; - return res; -#endif -} - -uint32_t rotl(uint32_t n, unsigned int c) { - const unsigned int mask = (CHAR_BIT*sizeof(n) - 1); - c &= mask; - return (n<>( (-c)&mask )); +void panicf(const char *fmt, ...) { + va_list args; + va_start(args, fmt); + vprintf(fmt, args); + va_end(args); + exit(1); } diff --git a/util.h b/util.h index 7c0b8e0..ecb8a93 100644 --- a/util.h +++ b/util.h @@ -1,9 +1,7 @@ // vi: set ft=c #ifndef UTIL_H #define UTIL_H -#include -unsigned long log2lu(unsigned long); -uint32_t rotl (uint32_t n, unsigned int c); +void panicf(const char *fmt, ...); #endif