From e11c8b5c4808b5648645b783e5f403f2af5d1e6a Mon Sep 17 00:00:00 2001 From: viandoxdev Date: Wed, 31 Aug 2022 18:54:14 +0200 Subject: [PATCH] pretty much done, just daemonizing left --- Makefile | 4 +- client.c | 113 ++++++++-- client.h | 4 +- hid.c | 14 +- hid.h | 4 +- json.c | 614 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ json.h | 68 ++++++ main.h | 4 +- net.c | 52 +++-- net.h | 22 +- server.c | 18 +- server.h | 4 +- util.h | 4 +- vec.h | 4 +- 14 files changed, 856 insertions(+), 73 deletions(-) create mode 100644 json.c create mode 100644 json.h diff --git a/Makefile b/Makefile index ec17a62..c6e1433 100644 --- a/Makefile +++ b/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 $<" diff --git a/client.c b/client.c index 9716509..10e6eef 100644 --- a/client.c +++ b/client.c @@ -1,5 +1,6 @@ #include "client.h" +#include "json.h" #include "net.h" #include "util.h" @@ -7,6 +8,7 @@ #include #include #include +#include #include #include #include @@ -14,11 +16,11 @@ #include #include #include +#include #include #include #include #include -#include 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,14 +97,14 @@ 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; - setup.code = dev->abs_id[i]; - setup.absinfo.minimum = dev->abs_min[i]; - setup.absinfo.maximum = dev->abs_max[i]; - setup.absinfo.fuzz = dev->abs_fuzz[i]; - setup.absinfo.flat = dev->abs_flat[i]; - setup.absinfo.resolution = dev->abs_res[i]; - setup.absinfo.value = 0; + struct uinput_abs_setup setup = {}; + setup.code = dev->abs_id[i]; + setup.absinfo.minimum = dev->abs_min[i]; + setup.absinfo.maximum = dev->abs_max[i]; + setup.absinfo.fuzz = dev->abs_fuzz[i]; + setup.absinfo.flat = dev->abs_flat[i]; + setup.absinfo.resolution = dev->abs_res[i]; + setup.absinfo.value = 0; ioctl(fd, UI_ABS_SETUP, &setup); } } @@ -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) { - // Reopen fifo - open_fifo(); - } else if (fifo_poll->revents & POLLIN) { + if (fifo_poll->revents & POLLIN || fifo_poll->revents & POLLHUP || fifo_poll->revents & POLLERR) { 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 - buf[len] = '\0'; - printf("CLIENT: Got fifo message:\n%s\n", buf); + int rc = json_parse((char *)buf, len, json_buf, 2048); + if (rc < 0) { + printf("CLIENT: Error when parsing fifo message as json (%s at index %lu)\n", + json_strerr(), json_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); + }; + }; + } } } diff --git a/client.h b/client.h index 896e4b5..9e069af 100644 --- a/client.h +++ b/client.h @@ -1,6 +1,6 @@ // vi:ft=c -#ifndef CLIENT_H -#define CLIENT_H +#ifndef CLIENT_H_ +#define CLIENT_H_ #include void client_run(char *address, uint16_t port); diff --git a/hid.c b/hid.c index dfac7f9..7819b41 100644 --- a/hid.c +++ b/hid.c @@ -64,11 +64,12 @@ void setup_device(PhysicalDevice *dev) { for (int i = 0; i < KEY_CNT; i++) dev->mapping.key_indices[i] = -1; - uint8_t bits[EV_MAX] = {}; - uint8_t feat_bits[KEY_MAX] = {}; + uint8_t bits[EV_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]; - snprintf(hidraw_path, 256, "/sys/class/input/%s/device/device/hidraw", input->d_name); + char hidraw_dir_path[256]; + snprintf(hidraw_dir_path, 256, "/sys/class/input/%s/device/device/hidraw", input->d_name); - DIR *hidraw_dir = opendir(hidraw_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; diff --git a/hid.h b/hid.h index dedf642..64d1999 100644 --- a/hid.h +++ b/hid.h @@ -1,6 +1,6 @@ // vi:ft=c -#ifndef HID_H -#define HID_H +#ifndef HID_H_ +#define HID_H_ #include "net.h" #include diff --git a/json.c b/json.c new file mode 100644 index 0000000..f5cc868 --- /dev/null +++ b/json.c @@ -0,0 +1,614 @@ +#define JSON_C_ +#include "json.h" + +#include +#include +#include +#include +#include +#include +#include + +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); +} diff --git a/json.h b/json.h new file mode 100644 index 0000000..4db5f16 --- /dev/null +++ b/json.h @@ -0,0 +1,68 @@ +// vi:ft=c +#ifndef JSON_H_ +#define JSON_H_ +#include +#include +#include +#include + +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 diff --git a/main.h b/main.h index dd8b9cd..683ff2f 100644 --- a/main.h +++ b/main.h @@ -1,5 +1,5 @@ // vi:ft=c -#ifndef MAIN_H -#define MAIN_H +#ifndef MAIN_H_ +#define MAIN_H_ #endif diff --git a/net.c b/net.c index 7ef8ed2..fcf6431 100644 --- a/net.c +++ b/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]; diff --git a/net.h b/net.h index 0982336..bfd01cb 100644 --- a/net.h +++ b/net.h @@ -1,6 +1,6 @@ // vi:ft=c -#ifndef NET_H -#define NET_H +#ifndef NET_H_ +#define NET_H_ #include #include #include @@ -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 -// 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); +int msg_deserialize(const uint8_t *buf, size_t len, Message * restrict dst); +int msg_serialize(uint8_t * restrict buf, size_t len, const Message *msg); #endif diff --git a/server.c b/server.c index d6b0e24..213846c 100644 --- a/server.c +++ b/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) diff --git a/server.h b/server.h index bfe0f89..d09e009 100644 --- a/server.h +++ b/server.h @@ -1,6 +1,6 @@ // vi:ft=c -#ifndef SERVER_H -#define SERVER_H +#ifndef SERVER_H_ +#define SERVER_H_ #include void server_run(uint16_t port); diff --git a/util.h b/util.h index 9fc1f62..704dc30 100644 --- a/util.h +++ b/util.h @@ -1,6 +1,6 @@ // vi:ft=c -#ifndef UTIL_H -#define UTIL_H +#ifndef UTIL_H_ +#define UTIL_H_ void panicf(const char *fmt, ...); diff --git a/vec.h b/vec.h index fe0b90d..a023777 100644 --- a/vec.h +++ b/vec.h @@ -1,6 +1,6 @@ // vi:ft=c -#ifndef VEC_H -#define VEC_H +#ifndef VEC_H_ +#define VEC_H_ #include #define vec_of(type) vec_new(sizeof(type))