never gonna give you up

hid.c

@@ -1,19 +1,18 @@
-#include<stdlib.h>
 #include <dirent.h>
-#include<string.h>
-#include<stdio.h>
-#include<sys/ioctl.h>
-#include<linux/input.h>
 #include <fcntl.h>
-#include<linux/uinput.h>
 #include <linux/input.h>
 #include <linux/joystick.h>
+#include <linux/uinput.h>
 #include <stdbool.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/ioctl.h>
 #include <time.h>
 
 #include "hid.h"
-#include "vec.h"
 #include "main.h"
+#include "vec.h"
 
 // List of uniq of the currently known devices
 static Vec devices;
@@ -36,16 +35,62 @@ uniq_t parse_uniq(char uniq[17]) {
     for (int i = 0; i < 17; i++) {
         char c = uniq[i];
         int  digit;
-        if(c >= '0' && c <= '9') digit = c - '0';
+        if (c >= '0' && c <= '9')
-        else if(c >= 'a' && c <= 'f') digit = c - 'a' + 10;
+            digit = c - '0';
-        else if(c >= 'A' && c <= 'F') digit = c - 'A' + 10;
+        else if (c >= 'a' && c <= 'f')
-        else continue;
+            digit = c - 'a' + 10;
+        else if (c >= 'A' && c <= 'F')
+            digit = c - 'A' + 10;
+        else
+            continue;
         res <<= 4;
         res += digit;
     }
     return res;
 }
 
+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);
@@ -131,7 +176,7 @@ void poll_devices() {
             continue;
         }
 
-        PhysicalDevice dev = {};
+        PhysicalDevice dev;
 
         char event_path[64];
         snprintf(event_path, 64, "/dev/input/%s", input->d_name);
@@ -149,26 +194,19 @@ void poll_devices() {
         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];
+            ioctl(dev.event, EVIOCGUNIQ(17), uniq_str);
-            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);
             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;
@@ -183,7 +221,8 @@ void poll_devices() {
         }
         pthread_mutex_unlock(&devices_mutex);
 
-        if(found) goto skip;
+        if (found)
+            goto skip;
 
         dev.uniq = uniq;
 
@@ -196,7 +235,8 @@ void poll_devices() {
             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 (strncmp(hidraw->d_name, "hidraw", 6) == 0)
+                    break;
             }
 
             if (hidraw == NULL) {
@@ -210,7 +250,8 @@ 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)
@@ -218,6 +259,8 @@ void poll_devices() {
         else
             strcpy(dev.name, name);
 
+        setup_device(&dev);
+
         pthread_mutex_lock(&devices_mutex);
         vec_push(&devices, &uniq);
         pthread_mutex_unlock(&devices_mutex);
@@ -242,6 +285,25 @@ skip:
     }
 }
 
+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();

hid.h

@@ -1,21 +1,32 @@
 // vi: set ft=c
 #ifndef HID_H
 #define HID_H
-#include<stdint.h>
+#include "net.h"
-#include<pthread.h>
 #include "vec.h"
+#include <linux/input.h>
+#include <pthread.h>
+#include <stdint.h>
 
 typedef uint64_t uniq_t;
 
+typedef struct {
+    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);
 PhysicalDevice get_device();
+void           apply_controller_state(PhysicalDevice *dev, MessageControllerState *state);
 
 #endif

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_t thread;
-    pthread_create(&_, NULL, hid_thread, NULL);
+    pthread_create(&thread, NULL, hid_thread, NULL);
-
     server_run(port);
 }
 

main.h

@@ -1,10 +1,5 @@
 // vi: set ft=c
 #ifndef MAIN_H
 #define MAIN_H
-#include<stdint.h>
-#include<pthread.h>
-#include "vec.h"
-
-void panicf(const char * fmt, ...);
 
 #endif

net.c

@@ -11,31 +11,63 @@ Message msg_device_info() {
 
 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;
 
     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;
+        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;
+        if (len < MSS_DEVICE_REPORT)
+            return -1;
         dst->code = code;
         return 0;
     case DeviceDestroy:
-            if(len < MSS_DEVICE_DESTROY) return -1;
+        if (len < MSS_DEVICE_DESTROY)
+            return -1;
         dst->code = code;
         return 0;
     case ControllerState:
-            if(len < MSS_CONTROLLER_STATE) return -1;
+        if (len < MSS_CONTROLLER_STATE)
+            return -1;
+
         dst->code                          = code;
         dst->controller_state.led[0]       = buf[1];
         dst->controller_state.led[1]       = buf[2];
@@ -51,36 +83,70 @@ int msg_deserialize(const uint8_t * buf, size_t len, Message * dst) {
 }
 
 // The indices have to match with msg_deserialize
-int msg_serialize(uint8_t * buf, size_t len, Message msg) {
+int msg_serialize(uint8_t *buf, size_t len, Message *msg) {
-    switch(msg.code) {
+    // If len is 0 we can't serialize any message
-        case Heartbeat:
+    if (len-- == 0)
-            if(MSS_HEARTBEAT >= len) return -1;
+        return -1;
-            buf[0] = (uint8_t) msg.code;
+
-            buf[1] = msg.heartbeat.alive;
+    switch (msg->code) {
-            return 0;
+    case DeviceInfo:; // semicolon needed here
-        case DeviceInfo:
+        uint8_t abs = msg->device_info.abs_count;
-            if(MSS_DEVICE_INFO >= len) return -1;
+        uint8_t rel = msg->device_info.rel_count;
-            buf[0] = (uint8_t) msg.code;
+        uint8_t key = msg->device_info.key_count;
-            return 0;
+        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(MSS_DEVICE_REPORT >= len) return -1;
+        if (len < MSS_DEVICE_REPORT)
-            buf[0] = (uint8_t) msg.code;
+            return -1;
-            return 0;
+
+        buf[0] = (uint8_t)msg->code;
+        return MSS_DEVICE_REPORT + 1;
     case DeviceDestroy:
-            if(MSS_DEVICE_DESTROY >= len) return -1;
+        if (len < MSS_DEVICE_DESTROY)
-            buf[0] = (uint8_t) msg.code;
+            return -1;
-            return 0;
+
+        buf[0] = (uint8_t)msg->code;
+        return MSS_DEVICE_DESTROY + 1;
     case ControllerState:
-            if(MSS_CONTROLLER_STATE >= len) return -1;
+        if (len < MSS_CONTROLLER_STATE)
-            buf[0] = (uint8_t) msg.code;
+            return -1;
-            buf[1] = msg.controller_state.led[0];
+
-            buf[2] = msg.controller_state.led[1];
+        buf[0] = (uint8_t)msg->code;
-            buf[3] = msg.controller_state.led[2];
+        buf[1] = msg->controller_state.led[0];
-            buf[4] = msg.controller_state.small_rumble;
+        buf[2] = msg->controller_state.led[1];
-            buf[5] = msg.controller_state.big_rumble;
+        buf[3] = msg->controller_state.led[2];
-            buf[6] = msg.controller_state.flash_on;
+        buf[4] = msg->controller_state.small_rumble;
-            buf[7] = msg.controller_state.flash_off;
+        buf[5] = msg->controller_state.big_rumble;
-            return 0;
+        buf[6] = msg->controller_state.flash_on;
+        buf[7] = msg->controller_state.flash_off;
+        return MSS_CONTROLLER_STATE + 1;
     default:
         return -1;
     }

net.h

@@ -1,11 +1,11 @@
 // vi: set ft=c
 #ifndef NET_H
 #define NET_H
+#include <linux/input.h>
 #include <stdint.h>
 #include <stdlib.h>
 
 typedef enum {
-    Heartbeat = 0,
     DeviceInfo      = 1,
     DeviceReport    = 2,
     DeviceDestroy   = 3,
@@ -14,17 +14,26 @@ typedef enum {
 
 typedef struct {
     MessageCode code;
-    uint8_t alive;
+
-} MessageHeartbeat;
+    uint8_t abs_count;
-#define MSS_HEARTBEAT 1
+    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;
@@ -37,6 +46,7 @@ typedef struct {
 
 typedef struct {
     MessageCode code;
+
     uint8_t led[3];
     uint8_t small_rumble;
     uint8_t big_rumble;
@@ -47,14 +57,17 @@ typedef struct  {
 
 typedef union {
     MessageCode            code;
-    MessageHeartbeat heartbeat;
     MessageDeviceInfo      device_info;
     MessageDeviceReport    device_report;
     MessageDeviceDestroy   device_destroy;
     MessageControllerState controller_state;
 } Message;
 
+// 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);
-int msg_serialize(uint8_t * buf, size_t len, Message msg);
+// 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

server.c

@@ -1,14 +1,19 @@
 #include <arpa/inet.h>
+#include <fcntl.h>
+#include <linux/input.h>
 #include <netinet/tcp.h>
 #include <pthread.h>
+#include <stdbool.h>
 #include <stdint.h>
 #include <stdio.h>
 #include <string.h>
+#include <sys/ioctl.h>
+#include <sys/poll.h>
 #include <sys/socket.h>
 
 #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);
+        int rc = poll(pfd, 1, -1);
+        if (rc < 0) // error (connection closed)
+            goto conn_end;
 
+        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("Couldn't parse message.\n");
+                    printf("CONN(%d): Illegal message\n", args->id);
+                }
+
+            } else {
+                printf("CONN(%d): Couldn't parse message.\n", args->id);
+            }
+        }
+        if (pfd[1].revents & POLLIN) {
+
         }
     }
-    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;

util.c

@@ -1,22 +1,18 @@
-#include "util.h"
 #include<limits.h>
+#include<stdarg.h>
+#include<stdio.h>
+#include<stdlib.h>
+
+#include "util.h"
 
 #ifndef __has_builtin
 #define __has_builtin(_) 0
 #endif
 
-unsigned long log2lu(unsigned long n) {
+void panicf(const char *fmt, ...) {
-#if __has_builtin(__builtin_clz)
+    va_list args;
-    return sizeof(unsigned long) * CHAR_BIT - __builtin_clz(n) - 1;
+    va_start(args, fmt);
-#else
+    vprintf(fmt, args);
-    unsigned long res = 0;
+    va_end(args);
-    while(n >>= 1) ++res;
+    exit(1);
-    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) | (n>>( (-c)&mask ));
 }

util.h

@@ -1,9 +1,7 @@
 // vi: set ft=c
 #ifndef UTIL_H
 #define UTIL_H
-#include <stdint.h>
 
-unsigned long log2lu(unsigned long);
+void panicf(const char *fmt, ...);
-uint32_t rotl (uint32_t n, unsigned int c);
 
 #endif