diff --git a/hid.c b/hid.c
index b21c219..dbff022 100644
--- a/hid.c
+++ b/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<stdbool.h>
-#include<time.h>
+#include <dirent.h>
+#include <fcntl.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;
@@ -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<stdint.h>
-#include<pthread.h>
+#include "net.h"
 #include "vec.h"
+#include <linux/input.h>
+#include <pthread.h>
+#include <stdint.h>
 
 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<stdint.h>
-#include<pthread.h>
-#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<stdint.h>
-#include<stdlib.h>
+#include <linux/input.h>
+#include <stdint.h>
+#include <stdlib.h>
 
 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 <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);
-
-        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<limits.h>
+#include<stdarg.h>
+#include<stdio.h>
+#include<stdlib.h>
+
 #include "util.h"
-#include <limits.h>
 
 #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) | (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 <stdint.h>
 
-unsigned long log2lu(unsigned long);
-uint32_t rotl (uint32_t n, unsigned int c);
+void panicf(const char *fmt, ...);
 
 #endif