pretty much done, just daemonizing left

Makefile

@@ -1,6 +1,6 @@
 Q=@
 CC=gcc
-CFLAGS=-g -Wall -Wno-format-truncation -pthread -DJSFW_DEV
+CFLAGS=-g -Wall -Wno-format-truncation -pthread -DJSFW_DEV -lm
 LDFLAGS=
 BUILD_DIR=./objects
 BIN=jsfw
@@ -19,7 +19,7 @@ run: $(BIN)
 
 $(BIN): $(OBJECTS)
 	@echo "LD    $@"
-	$(Q) $(CC) $(LDFLAGS) $^ -o $@
+	$(Q) $(CC) $(CFLAGS) $(LDFLAGS) $^ -o $@
 
 $(BUILD_DIR)/%.o: %.c | $(BUILD_DIR)
 	@echo "CC    $<"

client.c

@@ -1,5 +1,6 @@
 #include "client.h"
 
+#include "json.h"
 #include "net.h"
 #include "util.h"
 
@@ -7,6 +8,7 @@
 #include <fcntl.h>
 #include <linux/input.h>
 #include <linux/uinput.h>
+#include <math.h>
 #include <netinet/in.h>
 #include <poll.h>
 #include <stdbool.h>
@@ -14,11 +16,11 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <sys/ioctl.h>
 #include <sys/socket.h>
 #include <sys/stat.h>
 #include <time.h>
 #include <unistd.h>
-#include <sys/ioctl.h>
 
 typedef struct {
     int               fd;
@@ -55,6 +57,22 @@ static VirtualDevice device = {};
 
 static inline bool device_exists() { return device.fd > 0; }
 
+typedef struct {
+    char  *led_color;
+    double rumble_small;
+    double rumble_big;
+    double flash_on;
+    double flash_off;
+} JControllerState;
+
+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)},
+};
+
 void device_destroy() {
     if (!device_exists()) {
         return;
@@ -79,7 +97,7 @@ void device_init(MessageDeviceInfo *dev) {
     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;
+            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];
@@ -240,6 +258,17 @@ void early_checks() {
     close(fd);
 }
 
+static uint8_t parse_hex_digit(char h) {
+    if (h >= '0' && h <= '9')
+        return h - '0';
+    else if (h >= 'a' && h <= 'f')
+        return h - 'a' + 10;
+    else if (h >= 'A' && h <= 'F')
+        return h - 'A' + 10;
+    else
+        return 0;
+}
+
 void client_run(char *address, uint16_t port) {
     // Device doesn't exist yet
     device.fd = -1;
@@ -248,7 +277,8 @@ void client_run(char *address, uint16_t port) {
     setup_fifo();
     setup_server(address, port);
 
-    uint8_t buf[2049];
+    uint8_t buf[2048] __attribute__((aligned(4)));
+    uint8_t json_buf[2048] __attribute__((aligned(8)));
     while (1) {
         int rc = poll(poll_fds, 2, -1);
         if (rc < 0) {
@@ -256,21 +286,64 @@ void client_run(char *address, uint16_t port) {
             exit(1);
         }
 
-        if (fifo_poll->revents & POLLHUP || fifo_poll->revents & POLLERR) {
+        if (fifo_poll->revents & POLLIN || fifo_poll->revents & POLLHUP || fifo_poll->revents & POLLERR) {
-            // Reopen fifo
-            open_fifo();
-        } else if (fifo_poll->revents & POLLIN) {
             int len = read(fifo, buf, 2048);
             if (len <= 0) {
-                // This shouldn't ever happen as the poll already checks for the kind of error that would
-                // cause len to be <= 0
-                printf("CLIENT: supposedly unreachable code reached\n");
                 open_fifo();
             } else {
                 // We've got data from the fifo
-                // TODO: parse and handle that
+                int rc = json_parse((char *)buf, len, json_buf, 2048);
-                buf[len] = '\0';
+                if (rc < 0) {
-                printf("CLIENT: Got fifo message:\n%s\n", buf);
+                    printf("CLIENT: Error when parsing fifo message as json (%s at index %lu)\n",
+                           json_strerr(), json_err_loc());
+                } 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);
+                    }
+
+                    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);
+                        };
+                    };
+                }
             }
         }
 

client.h

@@ -1,6 +1,6 @@
 // vi:ft=c
-#ifndef CLIENT_H
+#ifndef CLIENT_H_
-#define CLIENT_H
+#define CLIENT_H_
 #include <stdint.h>
 
 void client_run(char *address, uint16_t port);

hid.c

@@ -65,10 +65,11 @@ void setup_device(PhysicalDevice *dev) {
         dev->mapping.key_indices[i] = -1;
 
     uint8_t bits[EV_MAX]                 = {};
-    uint8_t feat_bits[KEY_MAX] = {};
+    uint8_t feat_bits[(KEY_MAX + 7) / 8] = {};
     ioctl(dev->event, EVIOCGBIT(0, EV_MAX), bits);
     for (int i = 0; i < EV_MAX; i++) {
         if (bit_set(bits, i)) {
+            memset(feat_bits, 0, sizeof(feat_bits));
             ioctl(dev->event, EVIOCGBIT(i, KEY_MAX), feat_bits);
             for (int j = 0; j < KEY_MAX; j++) {
                 if (bit_set(feat_bits, j)) {
@@ -236,10 +237,10 @@ void poll_devices() {
         char hidraw_path[64];
 
         {
-            char hidraw_path[256];
+            char hidraw_dir_path[256];
-            snprintf(hidraw_path, 256, "/sys/class/input/%s/device/device/hidraw", input->d_name);
+            snprintf(hidraw_dir_path, 256, "/sys/class/input/%s/device/device/hidraw", input->d_name);
 
-            DIR           *hidraw_dir = opendir(hidraw_path);
+            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)
@@ -293,6 +294,9 @@ void poll_devices() {
 }
 
 void apply_controller_state(PhysicalDevice *dev, MessageControllerState *state) {
+    printf("HID:     Controller state: #%02x%02x%02x (%d, %d) rumble: (%d, %d)\n", 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};
 
     buf[4]  = state->small_rumble;

hid.h

@@ -1,6 +1,6 @@
 // vi:ft=c
-#ifndef HID_H
+#ifndef HID_H_
-#define HID_H
+#define HID_H_
 #include "net.h"
 
 #include <linux/input.h>

json.c

@@ -0,0 +1,614 @@
+#define JSON_C_
+#include "json.h"
+
+#include <math.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <unistd.h>
+#include <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+static JSONError jerrno = NoError;
+static size_t jerr_index = 0;
+
+const char * json_strerr() {
+    return JSONErrorMessage[jerrno];
+}
+
+size_t json_err_loc() {
+    return jerr_index;
+}
+
+// Shorthand to set jerno and return -1;
+static inline int set_jerrno(JSONError err) {
+    jerrno = err;
+    return -1;
+}
+static inline size_t align_8(size_t n) { return (((n - 1) >> 3) + 1) << 3; }
+static inline bool is_whitespace(char c) { return c == ' ' || c == '\t' || c == '\n'; }
+
+static int json_parse_value(const char **buf, const char *buf_end, uint8_t **restrict dst,
+                            const uint8_t *dst_end);
+
+// *dst must be 8 aligned
+static inline int json_parse_string(const char **buf, const char *buf_end, uint8_t **restrict dst,
+                                    const uint8_t *dst_end) {
+    if (*dst + sizeof(JSONHeader) >= dst_end) {
+        return set_jerrno(DstOverflow);
+    }
+
+    JSONHeader *header = (JSONHeader *)(*dst);
+    header->type       = (uint32_t)String;
+    header->len        = 0;
+    *dst += sizeof(JSONHeader);
+
+    // Skip first quote
+    (*buf)++;
+    if (*buf == buf_end) {
+        return set_jerrno(SrcOverflow);
+    }
+
+    // If the last char was an esc
+    bool esc = false;
+    // If we're currently parsing a unicode escape,
+    // -1: no, 0-4: we're n char in
+    int esc_unicode = -1;
+    // The unicode codepoint we're parsing
+    int un_codepoint = 0;
+
+    for (; *buf < buf_end; (*buf)++) {
+        char c = **buf;
+
+        if (esc_unicode >= 0) {
+            int 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 {
+                return set_jerrno(StringBadUnicode);
+            }
+            un_codepoint <<= 4;
+            un_codepoint += digit;
+            esc_unicode++;
+
+            if (esc_unicode == 4) { // UTF-8 Encoding
+                if (un_codepoint <= 0x7f) {
+                    if (*dst + 1 >= dst_end)
+                        return set_jerrno(DstOverflow);
+
+                    *(*dst)++ = un_codepoint;
+                    header->len++;
+                } else if (un_codepoint <= 0x7ff) {
+                    if (*dst + 2 >= dst_end)
+                        return set_jerrno(DstOverflow);
+
+                    *(*dst)++ = 0b11000000 | (un_codepoint >> 6 & 0b011111);
+                    *(*dst)++ = 0b10000000 | (un_codepoint >> 0 & 0b111111);
+                    header->len += 2;
+                } else if (un_codepoint <= 0xffff) {
+                    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);
+                    header->len += 3;
+                } else if (un_codepoint <= 0x10ffff) {
+                    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);
+                    header->len += 4;
+                } else {
+                    return set_jerrno(StringBadUnicode);
+                }
+                esc_unicode = -1;
+            }
+        } else if (esc) {
+            char r;
+            switch (c) {
+            case '"':
+            case '\\':
+            case '/': // For some reason you can escape a slash in JSON
+                r = c;
+                break;
+            case 'b':
+                r = '\b';
+                break;
+            case 'f':
+                r = '\f';
+                break;
+            case 'n':
+                r = '\n';
+                break;
+            case 'r':
+                r = '\r';
+                break;
+            case 't':
+                r = '\t';
+                break;
+            case 'u':
+                esc_unicode = 0;
+                break;
+            default:
+                return set_jerrno(StringBadEscape);
+            }
+
+            if (c != 'u') {
+                if (*dst + 1 >= dst_end) {
+                    return set_jerrno(DstOverflow);
+                }
+                *(*dst)++ = r;
+                header->len++;
+            }
+
+            esc = false;
+        } else {
+            if (c == '\\') {
+                esc = true;
+                continue;
+            } else if (c == '"') {
+                int padded_len = align_8(header->len);
+                if (*dst + (padded_len - header->len) >= dst_end)
+                    return set_jerrno(DstOverflow);
+                for (; padded_len > header->len; padded_len--)
+                    *(*dst)++ = '\0';
+                (*buf)++;
+                return 0;
+            } else if ((c < ' ' && c != '\t') || c == 0x7f) {
+                jerrno = StringBadChar;
+                return -1;
+            }
+            if (*dst + 1 >= dst_end)
+                return set_jerrno(DstOverflow);
+            *(*dst)++ = c;
+            header->len++;
+        }
+    }
+    // The only way to get out of the loop is if *buf >= buf_end
+    return set_jerrno(SrcOverflow);
+}
+
+// *dst must be 8 aligned
+static int json_parse_number(const char **buf, const char *buf_end, uint8_t **restrict dst,
+                             const uint8_t *dst_end) {
+
+    if (*dst + sizeof(JSONHeader) + sizeof(double) >= dst_end) {
+        return set_jerrno(DstOverflow);
+    }
+
+    JSONHeader *header = (JSONHeader *)(*dst);
+    double     *value  = (double *)((*dst) + sizeof(JSONHeader));
+    *dst += sizeof(JSONHeader) + sizeof(double);
+
+    header->type = (uint32_t)Number;
+    header->len  = sizeof(double);
+
+    double sign = 1.0;
+    if (**buf == '-') {
+        (*buf)++;
+        sign = -1.0;
+    }
+
+    if (*buf >= buf_end)
+        return set_jerrno(SrcOverflow);
+    if (**buf != '0') {
+        for (; *buf < buf_end; (*buf)++) {
+            char c = **buf;
+            if (c < '0' || c > '9') {
+                break;
+            }
+
+            *value *= 10.0;
+            *value += (double)(c - '0');
+        }
+    } else {
+        (*buf)++;
+    }
+
+    if (*buf < buf_end && **buf == '.') {
+        double place = 0.1;
+        (*buf)++; // Skip dot
+        if (*buf >= buf_end)
+            return set_jerrno(SrcOverflow);
+        if (**buf < '0' || **buf > '9')
+            return set_jerrno(NumberBadChar);
+
+        for (; *buf < buf_end; (*buf)++) {
+            char c = **buf;
+            if (c < '0' || c > '9')
+                break;
+            double digit = (double)(c - '0');
+            *value += digit * place;
+            place *= 0.1;
+        }
+    }
+
+    if (*buf < buf_end && (**buf == 'e' || **buf == 'E')) {
+        double exp      = 0.0;
+        double exp_sign = 1.0;
+
+        (*buf)++; // Skip e/E
+        if (*buf >= buf_end)
+            return set_jerrno(SrcOverflow);
+
+        if (**buf == '+' || **buf == '-') {
+            exp_sign = **buf == '-' ? -1.0 : 1.0;
+            (*buf)++;
+            if (*buf >= buf_end)
+                return set_jerrno(SrcOverflow);
+        }
+
+        for (; *buf < buf_end; (*buf)++) {
+            char c = **buf;
+            if (c < '0' || c > '9')
+                break;
+            exp *= 10;
+            exp += (double)(c - '0');
+        }
+
+        exp *= exp_sign;
+        *value *= pow(10.0, exp);
+    }
+
+    *value *= sign;
+
+    return 0;
+}
+
+// *dst must be 8 aligned
+static int json_parse_boolean(const char **buf, const char *buf_end, uint8_t **restrict dst,
+                              const uint8_t *dst_end) {
+
+    if (*dst + sizeof(JSONHeader) + 8 >= dst_end) {
+        return set_jerrno(DstOverflow);
+    }
+
+    JSONHeader *header = (JSONHeader *)(*dst);
+    uint64_t     *value  = (uint64_t *)((*dst) + sizeof(JSONHeader));
+    *dst += sizeof(JSONHeader) + 8;
+
+    header->type = (uint32_t)Boolean;
+    header->len  = 8;
+
+    if(**buf == 't') {
+        if(*buf + 4 > buf_end) return set_jerrno(SrcOverflow);
+        if(strncmp(*buf, "true", 4) != 0) {
+            return set_jerrno(BadKeyword);
+        }
+        *buf += 4;
+        *value = 1;
+    } else if(**buf == 'f') {
+        if(*buf + 5 > buf_end) return set_jerrno(SrcOverflow);
+        if(strncmp(*buf, "false", 5) != 0) {
+            return set_jerrno(BadKeyword);
+        }
+        *buf += 5;
+        *value = 0;
+    } else {
+        return set_jerrno(BadKeyword);
+    }
+    return 0;
+}
+
+// *dst must be 8 aligned
+static int json_parse_null(const char **buf, const char *buf_end, uint8_t **restrict dst,
+                              const uint8_t *dst_end) {
+
+    if (*dst + sizeof(JSONHeader) >= dst_end) {
+        return set_jerrno(DstOverflow);
+    }
+
+    JSONHeader *header = (JSONHeader *)(*dst);
+    *dst += sizeof(JSONHeader);
+
+    header->type = (uint32_t)Null;
+    header->len  = 0;
+
+    if(*buf + 4 > buf_end) return set_jerrno(SrcOverflow);
+    if(strncmp(*buf, "null", 4) != 0) {
+        return set_jerrno(BadKeyword);
+    }
+    *buf += 4;
+    return 0;
+}
+
+// *dst must be 8 aligned
+static int json_parse_array(const char **buf, const char *buf_end, uint8_t **restrict dst,
+                              const uint8_t *dst_end) {
+
+    if (*dst + sizeof(JSONHeader) >= dst_end) {
+        return set_jerrno(DstOverflow);
+    }
+
+    JSONHeader *header = (JSONHeader *)(*dst);
+    *dst += sizeof(JSONHeader);
+    uint8_t * dst_arr_start = *dst;
+
+    header->type = (uint32_t)Array;
+
+    (*buf)++; // Skip [
+    // skip initial whitespace
+    for(; *buf < buf_end && is_whitespace(**buf); (*buf)++);
+    if(*buf == buf_end) return set_jerrno(SrcOverflow);
+    if(**buf == ']') {
+        header->len = 0;
+        return 0;
+    }
+    while(1) {
+        if (json_parse_value(buf, buf_end, dst, dst_end) != 0) return -1;
+        for(; *buf < buf_end && is_whitespace(**buf); (*buf)++);
+        if (*buf == buf_end) return set_jerrno(SrcOverflow);
+        if(**buf == ',') {
+            (*buf)++;
+        } else if(**buf == ']') {
+            (*buf)++;
+            break;
+        } else {
+            return set_jerrno(BadChar);
+        }
+    }
+    header->len = *dst - dst_arr_start;
+    return 0;
+}
+
+// *dst must be 8 aligned
+static int json_parse_object(const char **buf, const char *buf_end, uint8_t **restrict dst,
+                              const uint8_t *dst_end) {
+
+    if (*dst + sizeof(JSONHeader) >= dst_end) {
+        return set_jerrno(DstOverflow);
+    }
+
+    JSONHeader *header = (JSONHeader *)(*dst);
+    *dst += sizeof(JSONHeader);
+    uint8_t * dst_obj_start = *dst;
+
+    header->type = (uint32_t)Object;
+
+    (*buf)++; // Skip {
+
+    for(; *buf < buf_end && is_whitespace(**buf); (*buf)++);
+    if(*buf == buf_end) return set_jerrno(SrcOverflow);
+    if(**buf == '}') {
+        header->len = 0;
+        return 0;
+    }
+
+    while(1) {
+        // Skip whitespace before key
+        for(; *buf < buf_end && is_whitespace(**buf); (*buf)++);
+        // Parse key
+        if (json_parse_string(buf, buf_end, dst, dst_end) != 0) return -1;
+        // Skip whitespace after key
+        for(; *buf < buf_end && is_whitespace(**buf); (*buf)++);
+        // There should be at least one char
+        if(*buf == buf_end) return set_jerrno(SrcOverflow);
+        // There should be a colon
+        if(**buf != ':') return set_jerrno(ObjectBadChar);
+        // Skip colon
+        (*buf)++;
+        // Parse value (takes char of whitespace)
+        if (json_parse_value(buf, buf_end, dst, dst_end) != 0) return -1;
+        // Skip whitespace after value
+        for(; *buf < buf_end && is_whitespace(**buf); (*buf)++);
+        // There should be at least one char (} or ,)
+        if (*buf == buf_end) return set_jerrno(SrcOverflow);
+        if(**buf == ',') {
+            (*buf)++;
+        } else if(**buf == '}') {
+            (*buf)++;
+            break;
+        } else {
+            return set_jerrno(BadChar);
+        }
+    }
+    header->len = *dst - dst_obj_start;
+    return 0;
+}
+
+// *dst must be 8 aligned
+static int json_parse_value(const char **buf, const char *buf_end, uint8_t **restrict dst,
+                            const uint8_t *dst_end) {
+    for (; *buf < buf_end; (*buf)++) {
+        if(is_whitespace(**buf)) continue;
+
+        switch (**buf) {
+        case '"':
+            return json_parse_string(buf, buf_end, dst, dst_end);
+        case '-':
+        case '0':
+        case '1':
+        case '2':
+        case '3':
+        case '4':
+        case '5':
+        case '6':
+        case '7':
+        case '8':
+        case '9':
+            return json_parse_number(buf, buf_end, dst, dst_end);
+        case '{':
+            return json_parse_object(buf, buf_end, dst, dst_end);
+        case '[':
+            return json_parse_array(buf, buf_end, dst, dst_end);
+        case 't':
+        case 'f':
+            return json_parse_boolean(buf, buf_end, dst, dst_end);
+        case 'n':
+            return json_parse_null(buf, buf_end, dst, dst_end);
+        default:
+            return set_jerrno(BadChar);
+        }
+    }
+    if (*buf == buf_end)
+        return set_jerrno(SrcOverflow);
+    return 0;
+}
+
+int json_parse(const char * src, size_t src_len, uint8_t * dst, size_t dst_len) {
+    memset(dst, 0, dst_len);
+    const char * buf = src;
+    const char * buf_end = src + src_len;
+    uint8_t * dst_end = dst + dst_len;
+    int rc = json_parse_value(&buf, buf_end, &dst, dst_end);
+    jerr_index = buf - src;
+    return rc;
+}
+
+void json_print_value(uint8_t ** buf) {
+    JSONHeader * header = (JSONHeader*) *buf;
+    *buf += sizeof(header);
+    switch(header->type) {
+        case String:
+            printf("\"%.*s\"", header->len, *buf);
+            *buf += align_8(header->len);
+            break;
+        case Number:
+            printf("%lf", *(double *)*buf);
+            *buf += sizeof(double);
+            break;
+        case Boolean: 
+            {
+                uint64_t value = *(uint64_t*)*buf;
+                if(value == 1) {
+                    printf("true");
+                } else if(value == 0) {
+                    printf("false");
+                } else {
+                    printf("(boolean) garbage");
+                }
+                *buf += 8;
+            }
+            break;
+        case Null:
+            printf("null");
+            break;
+        case Array:
+            {
+                uint8_t * end = *buf + header->len;
+                printf("[");
+                while(1) {
+                    json_print_value(buf);
+                    if(*buf < end) {
+                        printf(", ");
+                    } else {
+                        printf("]");
+                        break;
+                    }
+                }
+            }
+            break;
+        case Object:
+            {
+                uint8_t * end = *buf + header->len;
+                printf("{");
+                while(1) {
+                    json_print_value(buf);
+                    printf(":");
+                    json_print_value(buf);
+                    if(*buf < end) {
+                        printf(",");
+                    } else {
+                        printf("}");
+                        break;
+                    }
+                }
+            }
+            break;
+    }
+}
+
+struct Test {
+    double a;
+    char * b;
+};
+
+const JSONAdapter TestAdapter[] = {
+    {".a", Number, offsetof(struct Test, a)},
+    {".b", String, offsetof(struct Test, b)},
+};
+
+static void json_adapt_set(uint8_t * buf, JSONAdapter * adapters, size_t adapter_count, void * ptr, char * path) {
+    JSONHeader * header = (JSONHeader *)buf;
+    for(int i = 0; i < adapter_count; i++) {
+        if(strcmp(path, adapters[i].path) == 0 && header->type == adapters[i].type) {
+            void * p = ptr + adapters[i].offset;
+            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;
+            }
+        }
+    }
+}
+
+static void json_adapt_priv(uint8_t ** buf, JSONAdapter * adapters, size_t adapter_count, void * ptr, char * full_path, char * path) {
+    JSONHeader * header = (JSONHeader *)*buf;
+
+    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:
+            *buf += sizeof(JSONHeader);
+            break;
+        case Array:
+            {
+                *buf += sizeof(JSONHeader);
+                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);
+                }
+            }
+            break;
+        case Object:
+            {
+                *buf += sizeof(JSONHeader);
+                uint8_t * end = *buf + header->len;
+                while(*buf < end) {
+                    JSONHeader * key_header = (JSONHeader*)*buf;
+                    *buf += sizeof(JSONHeader);
+
+                    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);
+                }
+            }
+            break;
+    }
+}
+
+void json_adapt(uint8_t * buf, JSONAdapter * adapters, size_t adapter_count, void * ptr) {
+    char path[512] = ".";
+    json_adapt_priv(&buf, adapters, adapter_count, ptr, path, path);
+}

json.h

@@ -0,0 +1,68 @@
+// vi:ft=c
+#ifndef JSON_H_
+#define JSON_H_
+#include <stdint.h>
+#include <limits.h>
+#include <unistd.h>
+#include <stddef.h>
+
+typedef struct __attribute__((packed, aligned(8))) {
+    uint32_t type;
+    uint32_t len;
+} JSONHeader;
+
+_Static_assert(sizeof(JSONHeader) == 8, "JSONHeader size isn't 8 bytes");
+_Static_assert(sizeof(double) == 8, "double size isn't 8 bytes");
+_Static_assert(CHAR_BIT / sizeof(char) == 8, "Byte isn't 8 bit");
+
+typedef enum {
+    String  = 1,
+    Number  = 2,
+    Object  = 3,
+    Array   = 4,
+    Boolean = 5,
+    Null    = 6,
+} JSONType;
+
+typedef enum {
+    NoError          = 0,
+    DstOverflow      = 1,
+    SrcOverflow      = 2,
+    BadKeyword       = 3,
+    BadChar          = 4,
+    StringBadChar    = 5,
+    StringBadUnicode = 6,
+    StringBadEscape  = 7,
+    NumberBadChar    = 8,
+    ObjectBadChar    = 9,
+    JERRORNO_MAX     = 10
+} JSONError;
+
+#ifdef JSON_C_
+static const char * JSONErrorMessage[JERRORNO_MAX + 1] = {
+    "No error",
+    "Destination buffer is not big enough",
+    "Source buffer overflowed before parsing finished",
+    "Unknown keyword",
+    "Unexpected character",
+    "Unexpected character in string",
+    "Bad unicoded escape in string",
+    "Illegal escape in string",
+    "Unexpected character in number",
+    "Unexpected character in object",
+    "?"
+};
+#endif
+
+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);
+const char * json_strerr();
+size_t json_err_loc();
+
+#endif

main.h

@@ -1,5 +1,5 @@
 // vi:ft=c
-#ifndef MAIN_H
+#ifndef MAIN_H_
-#define MAIN_H
+#define MAIN_H_
 
 #endif

net.c

@@ -11,7 +11,9 @@ Message msg_device_info() {
     return s;
 }
 
-int msg_deserialize(const uint8_t *buf, size_t len, Message *dst) {
+// Deserialize the message in buf, buf must be at least 4 aligned. Returns -1 on error, otherwise returns 0
+// and writes result to dst
+int msg_deserialize(const uint8_t *buf, size_t len, Message * restrict dst) {
     // Decrement len so that it becomes the len of the data without the code.
     if (len-- < 1)
         return -1;
@@ -23,13 +25,14 @@ int msg_deserialize(const uint8_t *buf, size_t len, Message *dst) {
 
     switch (code) {
     case DeviceInfo:
-        if (len < 6)
+        if (len < 7)
             return -1;
-        buf16 = (uint16_t *)(buf + 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 += 7;
+        buf += 8;
         if (MSS_DEVICE_INFO(abs, rel, key) > len)
             return -1;
 
@@ -40,7 +43,8 @@ int msg_deserialize(const uint8_t *buf, size_t len, Message *dst) {
 
         // SOA in c but serialized as AOS
         for (int i = 0; i < abs; i++) {
-            uint32_t *buf32 = (uint32_t *)(buf + 2);
+            // buf + 4: 2 bytes for id and 2 bytes of padding
+            uint32_t *buf32 = (uint32_t *)(buf + 4);
 
             dst->device_info.abs_id[i]   = *(uint16_t *)buf;
             dst->device_info.abs_min[i]  = buf32[0];
@@ -49,7 +53,7 @@ int msg_deserialize(const uint8_t *buf, size_t len, Message *dst) {
             dst->device_info.abs_flat[i] = buf32[3];
             dst->device_info.abs_res[i]  = buf32[4];
 
-            buf += 22;
+            buf += 24;
         }
 
         for (int i = 0; i < rel; i++) {
@@ -64,14 +68,15 @@ int msg_deserialize(const uint8_t *buf, size_t len, Message *dst) {
 
         return 0;
     case DeviceReport:
-        if (len < 6)
+        if (len < 7)
             return -1;
 
-        buf16 = (uint16_t *)(buf + 1);
+        // 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 += 7;
+        buf += 8;
         if (len < MSS_DEVICE_REPORT(abs, rel, key))
             return -1;
 
@@ -112,7 +117,8 @@ int msg_deserialize(const uint8_t *buf, size_t len, Message *dst) {
     }
 }
 
-int msg_serialize(uint8_t *buf, size_t len, Message *msg) {
+// Serialize the message msg in buf, buf must be at least 4 aligned. Returns -1 on error (buf not big enough);
+int msg_serialize(uint8_t * restrict buf, size_t len, const Message *msg) {
     // If len is 0 we can't serialize any message
     if (len-- == 0)
         return -1;
@@ -127,15 +133,20 @@ int msg_serialize(uint8_t *buf, size_t len, Message *msg) {
         if (len < MSS_DEVICE_INFO(abs, rel, key))
             return -1;
 
+        // We begin 4 aligned
         buf[0]   = (uint8_t)msg->code;
-        buf16    = (uint16_t *)(buf + 1);
+        // 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 += 7;
+        buf += 8;
 
+        // Back to 4 aligned
         for (int i = 0; i < abs; i++) {
-            uint32_t *buf32 = (uint32_t *)(buf + 2);
+            // buf + 4: 2 bytes for id and 2 bytes of padding
+            uint32_t *buf32 = (uint32_t *)(buf + 4);
 
             *(uint16_t *)buf = msg->device_info.abs_id[i];
 
@@ -145,9 +156,9 @@ int msg_serialize(uint8_t *buf, size_t len, Message *msg) {
             buf32[3] = msg->device_info.abs_flat[i];
             buf32[4] = msg->device_info.abs_res[i];
 
-            buf += 22;
+            buf += 24;
         }
-
+        // Still 4 aligned
         for (int i = 0; i < rel; i++) {
             *(uint16_t *)buf = msg->device_info.rel_id[i];
             buf += 2;
@@ -167,22 +178,23 @@ int msg_serialize(uint8_t *buf, size_t len, Message *msg) {
             return -1;
 
         buf[0]   = (uint8_t)msg->code;
-        buf16    = (uint16_t *)(buf + 1);
+        // 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 += 7;
+        buf += 8;
-
+        // We're 4 aligned already
         for (int i = 0; i < abs; i++) {
             *(uint32_t *)buf = msg->device_report.abs[i];
             buf += 4;
         }
-
+        // Still 4 aligned
         for (int i = 0; i < rel; i++) {
             *(uint32_t *)buf = msg->device_report.rel[i];
             buf += 4;
         }
-
+        // Doesn't matter since we're writing individual bytes
         for (int i = 0; i < key; i++)
             *(buf++) = msg->device_report.key[i];
 

net.h

@@ -1,6 +1,6 @@
 // vi:ft=c
-#ifndef NET_H
+#ifndef NET_H_
-#define NET_H
+#define NET_H_
 #include <linux/input.h>
 #include <stdint.h>
 #include <stdlib.h>
@@ -12,14 +12,17 @@ typedef enum {
     ControllerState = 4,
 } MessageCode;
 
+// Alignment 4
 typedef struct {
     MessageCode code;
+    // + 1 byte of padding
 
     uint16_t abs_count;
     uint16_t rel_count;
     uint16_t key_count;
 
     uint16_t abs_id[ABS_CNT];
+    // + 2 bytes of padding per abs
     uint32_t abs_min[ABS_CNT];
     uint32_t abs_max[ABS_CNT];
     uint32_t abs_fuzz[ABS_CNT];
@@ -30,12 +33,14 @@ typedef struct {
 
     uint16_t key_id[KEY_CNT];
 } MessageDeviceInfo;
-#define MSS_DEVICE_INFO(abs, rel, key) (6 + abs * 22 + rel * 2 + key * 2)
+#define MSS_DEVICE_INFO(abs, rel, key) (8 + abs * 24 + rel * 2 + key * 2 + 1)
 // MSS -> Message Serialized Size:
 // Size of the data of the message when serialized (no alignment / padding)
 
+// 4 aligned
 typedef struct {
     MessageCode code;
+    // + 1 byte of padding
 
     uint16_t abs_count;
     uint16_t rel_count;
@@ -45,8 +50,9 @@ 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)
+#define MSS_DEVICE_REPORT(abs, rel, key) (6 + abs * 4 + rel * 4 + key * 1 + 1)
 
+// 1 aligned
 typedef struct {
     MessageCode code;
 
@@ -65,11 +71,7 @@ typedef union {
     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
+int msg_deserialize(const uint8_t *buf, size_t len, Message * restrict dst);
-// read
+int msg_serialize(uint8_t * restrict buf, size_t len, const Message *msg);
-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

server.c

@@ -39,12 +39,24 @@ 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];
+    uint8_t        buf[2048] __attribute__((aligned(4))) = {};
     PhysicalDevice dev = get_device();
     printf("CONN(%u): got device '%s'\n", args->id, dev.name);
 
+    char *closing_message = "";
+
     int len = msg_serialize(buf, 2048, (Message *)&dev.device_info);
-    write(args->socket, buf, len);
+    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];
@@ -62,8 +74,6 @@ void *server_handle_conn(void *args_) {
     report.rel_count = dev.device_info.rel_count;
     report.key_count = dev.device_info.key_count;
 
-    char *closing_message = "";
-
     while (1) {
         int rc = poll(pfds, 2, -1);
         if (rc < 0) { // error (connection closed)

server.h

@@ -1,6 +1,6 @@
 // vi:ft=c
-#ifndef SERVER_H
+#ifndef SERVER_H_
-#define SERVER_H
+#define SERVER_H_
 #include <stdint.h>
 
 void server_run(uint16_t port);

util.h

@@ -1,6 +1,6 @@
 // vi:ft=c
-#ifndef UTIL_H
+#ifndef UTIL_H_
-#define UTIL_H
+#define UTIL_H_
 
 void panicf(const char *fmt, ...);
 

vec.h

@@ -1,6 +1,6 @@
 // vi:ft=c
-#ifndef VEC_H
+#ifndef VEC_H_
-#define VEC_H
+#define VEC_H_
 #include <unistd.h>
 
 #define vec_of(type) vec_new(sizeof(type))