stupidcomputer
/
jsfw
mirror of https://github.com/stupidcomputer/jsfw.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

rework ! just need testing

This commit is contained in:
viandoxdev 2022-10-08 01:36:53 +02:00
parent e371ef3ec4
commit ba38bf705a
No known key found for this signature in database
GPG Key ID: AF1410C5BC10AA25
21 changed files with 1447 additions and 528 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.clang-format
View File

@ -3,6 +3,7 @@ BasedOnStyle: LLVM
IndentWidth: 4 IndentWidth: 4
AlignConsecutiveDeclarations: true AlignConsecutiveDeclarations: true
AlignConsecutiveAssignments: true AlignConsecutiveAssignments: true
AlignArrayOfStructures: Left
PointerAlignment: Right PointerAlignment: Right
ColumnLimit: 110 ColumnLimit: 130
IncludeBlocks: Regroup IncludeBlocks: Regroup

4
Makefile
View File

@ -1,11 +1,11 @@
Q=@ Q=@
CC=gcc CC=gcc
CFLAGS=-g -Wall -Wno-format-truncation -pthread -lm CFLAGS=-std=c11 -pedantic -g -Wall -Wno-format-truncation -pthread -lm -D_GNU_SOURCE
LDFLAGS=-lm LDFLAGS=-lm
BUILD_DIR=./objects BUILD_DIR=./objects
BIN=jsfw BIN=jsfw
RUNARGS=client localhost 7776 RUNARGS=server 7776 ./server_config.json
SOURCES=$(wildcard *.c) SOURCES=$(wildcard *.c)

350
client.c
View File

@ -1,9 +1,9 @@
#include "client.h" #include "client.h"
#include "const.h" #include "const.h"
#include "json.h" #include "json.h"
#include "net.h" #include "net.h"
#include "util.h" #include "util.h"
#include "vec.h"
#include <arpa/inet.h> #include <arpa/inet.h>
#include <fcntl.h> #include <fcntl.h>
@ -25,17 +25,10 @@
#include <time.h> #include <time.h>
#include <unistd.h> #include <unistd.h>
// The current device.
// The fd being -1 means there is none
typedef struct {
int fd;
MessageDeviceInfo info;
} VirtualDevice;
static int fifo_attempt = 0; static int fifo_attempt = 0;
static struct sockaddr_in server_addr = {}; static struct sockaddr_in server_addr = {0};
static char server_addrp[64] = {}; static char server_addrp[64] = {0};
static uint16_t server_port = -1; static uint16_t server_port = -1;
static struct pollfd poll_fds[2]; static struct pollfd poll_fds[2];
@ -44,65 +37,122 @@ static struct pollfd *socket_poll = &poll_fds[1];
static int fifo = -1; static int fifo = -1;
static int sock = -1; static int sock = -1;
// static to avoid having this on the stack because a message is about 2kb in memory // static to avoid having this on the stack because a message is about 2kb in memory
static Message message; static Message message;
static VirtualDevice device = {};
// Test if the device exists static Vec devices_fd;
static inline bool device_exists() { return device.fd > 0; } static Vec devices_info;
// Struct representing the received json static ClientConfig config;
typedef struct { static MessageRequest device_request;
char *led_color;
double rumble_small;
double rumble_big;
double flash_on;
double flash_off;
} JControllerState;
static const JSONAdapter JControllerStateAdapter[] = { static void default_fifo_path(void *ptr) { *(char **)ptr = (char *)FIFO_PATH; }
{".led_color", String, offsetof(JControllerState, led_color)}, static void default_retry_delay(void *ptr) { *(struct timespec *)ptr = CONNECTION_RETRY_DELAY; }
{".rumble.0", Number, offsetof(JControllerState, rumble_small)}, static void default_vendor(void *ptr) { *(int32_t *)ptr = VIRTUAL_DEVICE_VENDOR; }
{".rumble.1", Number, offsetof(JControllerState, rumble_big)}, static void default_product(void *ptr) { *(int32_t *)ptr = VIRTUAL_DEVICE_PRODUCT; }
{".flash.0", Number, offsetof(JControllerState, flash_on)}, static void default_name(void *ptr) { *(char **)ptr = (char *)VIRTUAL_DEVICE_NAME; }
{".flash.1", Number, offsetof(JControllerState, flash_off)}, static void default_to_white(void *ptr) {
uint8_t *color = ptr;
color[0] = 255;
color[1] = 255;
color[2] = 255;
}
static const JSONPropertyAdapter ControllerStateAdapterProps[] = {
{".led_color", &StringAdapter, offsetof(MessageControllerState, led), default_to_white, tsf_hex_to_color },
{".rumble.0", &NumberAdapter, offsetof(MessageControllerState, small_rumble), default_to_zero_u8, tsf_num_to_u8_clamp},
{".rumble.1", &NumberAdapter, offsetof(MessageControllerState, big_rumble), default_to_zero_u8, tsf_num_to_u8_clamp},
{".flash.0", &NumberAdapter, offsetof(MessageControllerState, flash_on), default_to_zero_u8, tsf_num_to_u8_clamp},
{".flash.1", &NumberAdapter, offsetof(MessageControllerState, flash_off), default_to_zero_u8, tsf_num_to_u8_clamp},
{".index", &NumberAdapter, offsetof(MessageControllerState, index), default_to_zero_u32, tsf_num_to_int }
};
static const JSONAdapter ControllerStateAdapter = {
.props = (JSONPropertyAdapter *)ControllerStateAdapterProps,
.prop_count = sizeof(ControllerStateAdapterProps) / sizeof(JSONPropertyAdapter),
.size = sizeof(MessageControllerState),
}; };
// Try to destroy the device static const JSONPropertyAdapter ControllerAdapterProps[] = {
void device_destroy() { {".tag", &StringAdapter, offsetof(ClientController, tag), default_to_null, NULL },
if (!device_exists()) { {".vendor", &StringAdapter, offsetof(ClientController, device_vendor), default_vendor, tsf_hex_to_i32},
{".product", &StringAdapter, offsetof(ClientController, device_product), default_product, tsf_hex_to_i32},
{".name", &StringAdapter, offsetof(ClientController, device_name), default_name, NULL },
};
static const JSONAdapter ControllerAdapter = {
.props = ControllerAdapterProps,
.prop_count = sizeof(ControllerAdapterProps) / sizeof(JSONPropertyAdapter),
.size = sizeof(ClientController),
};
static const JSONPropertyAdapter ClientConfigAdapterProps[] = {
{".controllers[]", &ControllerAdapter, offsetof(ClientConfig, controllers), default_to_null, NULL },
{".fifo_path", &StringAdapter, offsetof(ClientConfig, fifo_path), default_fifo_path, NULL },
{".retry_delay", &NumberAdapter, offsetof(ClientConfig, retry_delay), default_retry_delay, tsf_numsec_to_timespec}
};
static const JSONAdapter ConfigAdapter = {
.props = ClientConfigAdapterProps,
.prop_count = sizeof(ClientConfigAdapterProps) / sizeof(JSONPropertyAdapter),
.size = sizeof(ClientConfig),
};
void destroy_devices() {
for (int i = 0; i < config.controller_count; i++) {
int fd = *(int *)vec_get(&devices_fd, i);
MessageDeviceInfo *info = vec_get(&devices_info, i);
if (info->code == DeviceInfo) {
ioctl(fd, UI_DEV_DESTROY);
info->code = NoMessage;
}
}
}
bool device_exists(int index) {
if (index >= devices_info.len) {
return false;
}
MessageDeviceInfo *info = vec_get(&devices_info, index);
return info->code == DeviceInfo;
}
void device_destroy(int index) {
if (index >= devices_info.len) {
return; return;
} }
ioctl(device.fd, UI_DEV_DESTROY); int fd = *(int *)vec_get(&devices_fd, index);
close(device.fd);
device.fd = -1; MessageDeviceInfo *info = vec_get(&devices_info, index);
printf("CLIENT: Destroyed device\n");
if (info->code == DeviceInfo) {
ioctl(fd, UI_DEV_DESTROY);
info->code = NoMessage;
}
} }
// (Re)Initialize the device
void device_init(MessageDeviceInfo *dev) { void device_init(MessageDeviceInfo *dev) {
device_destroy(); if (dev->index >= devices_info.len) {
printf("CLIENT: Got wrong device index\n");
int fd = open("/dev/uinput", O_WRONLY | O_NONBLOCK); return;
if (fd < 0) {
perror("CLIENT: Error while opening /dev/uinput, ");
exit(1);
} }
// Setup device_info device_destroy(dev->index);
int fd = *(int*)vec_get(&devices_fd, dev->index);
// Abs // Abs
if (dev->abs_count > 0) { if (dev->abs_count > 0) {
ioctl(fd, UI_SET_EVBIT, EV_ABS); ioctl(fd, UI_SET_EVBIT, EV_ABS);
for (int i = 0; i < dev->abs_count; i++) { for (int i = 0; i < dev->abs_count; i++) {
struct uinput_abs_setup setup = {}; struct uinput_abs_setup setup = {0};
setup.code = dev->abs_id[i];
setup.absinfo.minimum = dev->abs_min[i]; setup.code = dev->abs_id[i];
setup.absinfo.maximum = dev->abs_max[i]; setup.absinfo.minimum = dev->abs_min[i];
setup.absinfo.fuzz = dev->abs_fuzz[i]; setup.absinfo.maximum = dev->abs_max[i];
setup.absinfo.flat = dev->abs_flat[i]; setup.absinfo.fuzz = dev->abs_fuzz[i];
setup.absinfo.resolution = dev->abs_res[i]; setup.absinfo.flat = dev->abs_flat[i];
setup.absinfo.value = 0; setup.absinfo.resolution = dev->abs_res[i];
setup.absinfo.value = 0;
ioctl(fd, UI_ABS_SETUP, &setup); ioctl(fd, UI_ABS_SETUP, &setup);
} }
} }
@ -123,67 +173,95 @@ void device_init(MessageDeviceInfo *dev) {
} }
} }
struct uinput_setup setup = {}; ClientController *ctr = &config.controllers[dev->index];
struct uinput_setup setup = {0};
setup.id.bustype = BUS_VIRTUAL; setup.id.bustype = BUS_VIRTUAL;
setup.id.vendor = VIRTUAL_DEVICE_VENDOR; setup.id.vendor = ctr->device_vendor;
setup.id.product = VIRTUAL_DEVICE_PRODUCT; setup.id.product = ctr->device_product;
setup.id.version = VIRTUAL_DEVICE_VERSION; setup.id.version = VIRTUAL_DEVICE_VERSION;
strncpy(setup.name, VIRTUAL_DEVICE_NAME, UINPUT_MAX_NAME_SIZE); strncpy(setup.name, ctr->device_name, UINPUT_MAX_NAME_SIZE);
ioctl(fd, UI_DEV_SETUP, &setup); ioctl(fd, UI_DEV_SETUP, &setup);
ioctl(fd, UI_DEV_CREATE); ioctl(fd, UI_DEV_CREATE);
device.fd = fd; MessageDeviceInfo * dst = vec_get(&devices_info, dev->index);
memcpy(&device.info, dev, sizeof(MessageDeviceInfo));
printf("CLIENT: Created device (abs: %d, rel: %d, key: %d)\n", dev->abs_count, dev->rel_count, memcpy(dst, dev, sizeof(MessageDeviceInfo));
dev->key_count); printf("CLIENT: Got device [%d]: '%s' (abs: %d, rel: %d, key: %d)\n", dev->index, ctr->device_name, dev->abs_count, dev->rel_count, dev->key_count);
} }
// Send an event to uinput, device must exist // Send an event to uinput, device must exist
int device_emit(uint16_t type, uint16_t id, uint32_t value) { bool device_emit(int index, uint16_t type, uint16_t id, uint32_t value) {
struct input_event event = {}; if(index >= devices_fd.len) {
return true;
}
int fd = *(int*) vec_get(&devices_fd, index);
struct input_event event = {0};
event.type = type; event.type = type;
event.code = id; event.code = id;
event.value = value; event.value = value;
return write(device.fd, &event, sizeof(event)) != sizeof(event); return write(fd, &event, sizeof(event)) != sizeof(event);
} }
// Update device with report // Update device with report
void device_handle_report(MessageDeviceReport *report) { void device_handle_report(MessageDeviceReport *report) {
if (!device_exists()) { if (!device_exists(report->index)) {
printf("CLIENT: Got report before device info\n"); printf("CLIENT: [%d] Got report before device info\n", report->index);
return; return;
} }
if (report->abs_count != device.info.abs_count || report->rel_count != device.info.rel_count || MessageDeviceInfo * info = vec_get(&devices_info, report->index);
report->key_count != device.info.key_count) {
if (report->abs_count != info->abs_count || report->rel_count != info->rel_count ||
report->key_count != info->key_count) {
printf("CLIENT: Report doesn't match with device info\n"); printf("CLIENT: Report doesn't match with device info\n");
return; return;
} }
for (int i = 0; i < report->abs_count; i++) { for (int i = 0; i < report->abs_count; i++) {
if (device_emit(EV_ABS, device.info.abs_id[i], report->abs[i]) != 0) { if (device_emit(report->index, EV_ABS, info->abs_id[i], report->abs[i]) != 0) {
printf("CLIENT: Error writing abs event to uinput\n"); printf("CLIENT: Error writing abs event to uinput\n");
} }
} }
for (int i = 0; i < report->rel_count; i++) { for (int i = 0; i < report->rel_count; i++) {
if (device_emit(EV_REL, device.info.rel_id[i], report->rel[i]) != 0) { if (device_emit(report->index, EV_REL, info->rel_id[i], report->rel[i]) != 0) {
printf("CLIENT: Error writing rel event to uinput\n"); printf("CLIENT: Error writing rel event to uinput\n");
} }
} }
for (int i = 0; i < report->key_count; i++) { for (int i = 0; i < report->key_count; i++) {
if (device_emit(EV_KEY, device.info.key_id[i], (uint32_t)(!report->key[i]) - 1) != 0) { if (device_emit(report->index, EV_KEY, info->key_id[i], (uint32_t)(!report->key[i]) - 1) != 0) {
printf("CLIENT: Error writing key event to uinput\n"); printf("CLIENT: Error writing key event to uinput\n");
} }
} }
// Reports are sent by the server every time the server receives an EV_SYN from the physical device, so we // Reports are sent by the server every time the server receives an EV_SYN from the physical device, so we
// send one when we receive the report to match // send one when we receive the report to match
device_emit(EV_SYN, 0, 0); device_emit(report->index, EV_SYN, 0, 0);
}
void setup_devices() {
devices_fd = vec_of(int);
devices_info = vec_of(MessageDeviceInfo);
MessageDeviceInfo no_info = {0};
no_info.code = NoMessage;
for (int i = 0; i < config.controller_count; i++) {
int fd = open("/dev/uinput", O_WRONLY | O_NONBLOCK);
if (fd < 0) {
perror("CLIENT: Can't open /dev/uinput, aborting now");
exit(1);
}
vec_push(&devices_fd, &fd);
vec_push(&devices_info, &no_info);
}
} }
void setup_fifo(); void setup_fifo();
@ -191,10 +269,10 @@ void setup_fifo();
// (Re)Open the fifo // (Re)Open the fifo
void open_fifo() { void open_fifo() {
close(fifo); close(fifo);
fifo = open(FIFO_PATH, O_RDONLY | O_NONBLOCK); fifo = open(config.fifo_path, O_RDONLY | O_NONBLOCK);
if (fifo < 0 && fifo_attempt == 0) { if (fifo < 0 && fifo_attempt == 0) {
fifo_attempt++; fifo_attempt++;
unlink(FIFO_PATH); unlink(config.fifo_path);
setup_fifo(); setup_fifo();
} else if (fifo < 0) { } else if (fifo < 0) {
panicf("CLIENT: Couldn't open fifo, aborting\n"); panicf("CLIENT: Couldn't open fifo, aborting\n");
@ -205,7 +283,7 @@ void open_fifo() {
// Ensure the fifo exists and opens it (also setup poll_fd) // Ensure the fifo exists and opens it (also setup poll_fd)
void setup_fifo() { void setup_fifo() {
mode_t prev = umask(0); mode_t prev = umask(0);
mkfifo(FIFO_PATH, 0666); mkfifo(config.fifo_path, 0666);
umask(prev); umask(prev);
open_fifo(); open_fifo();
@ -216,11 +294,11 @@ void setup_fifo() {
// (Re)Connect to the server // (Re)Connect to the server
void connect_server() { void connect_server() {
while (1) { while (true) {
if (sock > 0) { if (sock > 0) {
// Close previous connection // Close previous connection
device_destroy();
shutdown(sock, SHUT_RDWR); shutdown(sock, SHUT_RDWR);
destroy_devices();
close(sock); close(sock);
} }
@ -230,11 +308,9 @@ void connect_server() {
} }
if (connect(sock, (struct sockaddr *)&server_addr, sizeof(server_addr)) != 0) { if (connect(sock, (struct sockaddr *)&server_addr, sizeof(server_addr)) != 0) {
printf("CLIENT: Couldn't connect to %s:%d, retrying in %ds\n", server_addrp, server_port, printf("CLIENT: Couldn't connect to %s:%d, retrying in %lu.%09lus\n", server_addrp, server_port,
CONNECTION_RETRY_DELAY); config.retry_delay.tv_sec, config.retry_delay.tv_nsec);
struct timespec ts = {}; nanosleep(&config.retry_delay, NULL);
ts.tv_sec = CONNECTION_RETRY_DELAY;
nanosleep(&ts, NULL);
continue; continue;
} }
// Set non blocking, only do that after connection (instead of with SOCK_NONBLOCK at socket creation) // Set non blocking, only do that after connection (instead of with SOCK_NONBLOCK at socket creation)
@ -242,6 +318,16 @@ void connect_server() {
fcntl(sock, F_SETFL, fcntl(sock, F_GETFL, 0) | O_NONBLOCK); fcntl(sock, F_SETFL, fcntl(sock, F_GETFL, 0) | O_NONBLOCK);
socket_poll->fd = sock; socket_poll->fd = sock;
printf("CLIENT: Connected !\n"); printf("CLIENT: Connected !\n");
uint8_t buf[2048] __attribute__((aligned(4)));
int len = msg_serialize(buf, 2048, (Message *)&device_request);
if (len > 0) {
if (send(sock, buf, len, 0) > 0) {
printf("CLIENT: Sent device request\n");
};
};
return; return;
} }
} }
@ -264,22 +350,45 @@ void setup_server(char *address, uint16_t port) {
connect_server(); connect_server();
} }
void early_checks() { void build_device_request() {
int fd = open("/dev/uinput", O_WRONLY | O_NONBLOCK); char **tags = malloc(config.controller_count * sizeof(char *));
if (fd < 0) { for (int i = 0; i < config.controller_count; i++) {
perror("CLIENT: Can't open /dev/uinput, aborting now: "); tags[i] = config.controllers[i].tag;
exit(1);
} }
close(fd);
device_request.code = Request;
device_request.request_count = config.controller_count;
device_request.requests = tags;
} }
void client_run(char *address, uint16_t port) { void client_run(char *address, uint16_t port, char *config_path) {
// Device doesn't exist yet // Parse the config
device.fd = -1; {
FILE *configfd = fopen(config_path, "r");
if (configfd == NULL) {
perror("CLIENT: Couldn't open config file");
exit(1);
}
char *cbuf = malloc(8192);
uint8_t *jbuf = (uint8_t *)cbuf + 4096;
int len = fread(cbuf, 1, 4096, configfd);
if (json_parse(cbuf, len, jbuf, 4096) != 0) {
printf("CLIENT: Couldn't parse config, %s (at index %lu)\n", json_strerr(), json_errloc());
exit(1);
}
json_adapt(jbuf, &ConfigAdapter, &config);
free(cbuf);
fclose(configfd);
}
early_checks();
setup_fifo(); setup_fifo();
build_device_request();
setup_server(address, port); setup_server(address, port);
setup_devices();
uint8_t buf[2048] __attribute__((aligned(4))); uint8_t buf[2048] __attribute__((aligned(4)));
uint8_t json_buf[2048] __attribute__((aligned(8))); uint8_t json_buf[2048] __attribute__((aligned(8)));
@ -287,7 +396,7 @@ void client_run(char *address, uint16_t port) {
while (1) { while (1) {
int rc = poll(poll_fds, 2, -1); int rc = poll(poll_fds, 2, -1);
if (rc < 0) { if (rc < 0) {
perror("CLIENT: Error on poll, "); perror("CLIENT: Error on poll");
exit(1); exit(1);
} }
@ -299,55 +408,19 @@ void client_run(char *address, uint16_t port) {
// We've got data from the fifo // We've got data from the fifo
int rc = json_parse((char *)buf, len, json_buf, 2048); int rc = json_parse((char *)buf, len, json_buf, 2048);
if (rc < 0) { if (rc < 0) {
printf("CLIENT: Error when parsing fifo message as json (%s at index %lu)\n", printf("CLIENT: Error when parsing fifo message as json (%s at index %lu)\n", json_strerr(), json_errloc());
json_strerr(), json_errloc());
} else { } 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; MessageControllerState msg;
msg.code = ControllerState; msg.code = ControllerState;
msg.small_rumble = (uint8_t)(fmax(fmin(1.0, state.rumble_small), 0.0) * 255.0); json_adapt(json_buf, &ControllerStateAdapter, &msg);
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); int len = msg_serialize(buf, 2048, (Message *)&msg);
if (len > 0) { if (len > 0) {
if (send(sock, buf, len, 0) > 0) { if (send(sock, buf, len, 0) > 0) {
printf("CLIENT: Sent controller state: #%02x%02x%02x flash: (%d, %d) rumble: " printf("CLIENT: Sent controller state: #%02x%02x%02x flash: (%d, %d) rumble: "
"(%d, %d)\n", "(%d, %d) -> [%d]\n",
msg.led[0], msg.led[1], msg.led[2], msg.flash_on, msg.flash_off, msg.led[0], msg.led[1], msg.led[2], msg.flash_on, msg.flash_off, msg.small_rumble,
msg.small_rumble, msg.big_rumble); msg.big_rumble, msg.index);
}; };
}; };
} }
@ -359,9 +432,8 @@ void client_run(char *address, uint16_t port) {
int len = recv(sock, buf, 2048, 0); int len = recv(sock, buf, 2048, 0);
if (len <= 0) { if (len <= 0) {
printf("CLIENT: Lost connection to server, reconnecting\n"); printf("CLIENT: Lost connection to server, reconnecting\n");
shutdown(sock, SHUT_RDWR);
connect_server(); connect_server();
// we can continue here because there's nothing after, unlike above for fifo (this reduces // we can use continue here because there's nothing after, unlike above for fifo (this reduces
// indentation instead of needing an else block) // indentation instead of needing an else block)
continue; continue;
} }
@ -384,13 +456,17 @@ void client_run(char *address, uint16_t port) {
} }
if (message.code == DeviceInfo) { if (message.code == DeviceInfo) {
if (device_exists()) { if (device_exists(message.device_info.index)) {
printf("CLIENT: Got more than one device info\n"); printf("CLIENT: Got more than one device info for same device\n");
} }
device_init((MessageDeviceInfo *)&message); device_init((MessageDeviceInfo *)&message);
printf("CLIENT: Got device %d\n", message.device_info.index);
} else if (message.code == DeviceReport) { } else if (message.code == DeviceReport) {
device_handle_report((MessageDeviceReport *)&message); device_handle_report((MessageDeviceReport *)&message);
} else if (message.code == DeviceDestroy) {
device_destroy(message.destroy.index);
printf("CLIENT: Lost device %d\n", message.destroy.index);
} else { } else {
printf("CLIENT: Illegal message\n"); printf("CLIENT: Illegal message\n");
} }

18
client.h
View File

@ -2,7 +2,23 @@
#ifndef CLIENT_H_ #ifndef CLIENT_H_
#define CLIENT_H_ #define CLIENT_H_
#include <stdint.h> #include <stdint.h>
#include <time.h>
void client_run(char *address, uint16_t port); void client_run(char *address, uint16_t port, char *config_path);
typedef struct {
char *tag;
int32_t device_vendor;
int32_t device_product;
char *device_name;
} ClientController;
typedef struct {
ClientController *controllers;
size_t controller_count;
char *fifo_path;
struct timespec retry_delay;
} ClientConfig;
#endif #endif

18
client_config.json Normal file
View File

@ -0,0 +1,18 @@
{
"controllers": [
{
"tag": "Admin",
"vendor": "054c",
"product": "09cc",
"name": "JSFW PS4 Controller (Admin)"
},
{
"tag": "User",
"vendor": "6942",
"product": "1337",
"name": "JSFW PS4 Controller (User)"
}
],
"fifo_path": "/tmp/jsfw_fifo_2",
"retry_delay": 2.5
}

4
const.c
View File

@ -5,12 +5,14 @@
// How long between each device poll // How long between each device poll
const struct timespec POLL_DEVICE_INTERVAL = {.tv_sec = 1, .tv_nsec = 0}; const struct timespec POLL_DEVICE_INTERVAL = {.tv_sec = 1, .tv_nsec = 0};
// How long (in ms) to wait for a request message on a connection before giving up
const int REQUEST_TIMEOUT = 2000;
// Default name for physical device, only visible in logs // Default name for physical device, only visible in logs
const char *DEVICE_DEFAULT_NAME = "Unnamed Device"; const char *DEVICE_DEFAULT_NAME = "Unnamed Device";
// Path to the fifo // Path to the fifo
const char *FIFO_PATH = "/tmp/jsfw_fifo"; const char *FIFO_PATH = "/tmp/jsfw_fifo";
// Delay (in seconds) between each connection retry for the client // Delay (in seconds) between each connection retry for the client
const uint32_t CONNECTION_RETRY_DELAY = 5; const struct timespec CONNECTION_RETRY_DELAY = {.tv_sec = 5, .tv_nsec = 0};
// Displayed vendor for the virtual device // Displayed vendor for the virtual device
const uint16_t VIRTUAL_DEVICE_VENDOR = 0x6969; const uint16_t VIRTUAL_DEVICE_VENDOR = 0x6969;
// Displayed product for the virtual device // Displayed product for the virtual device

3
const.h
View File

@ -5,9 +5,10 @@
#include <time.h> #include <time.h>
extern const struct timespec POLL_DEVICE_INTERVAL; extern const struct timespec POLL_DEVICE_INTERVAL;
extern const int REQUEST_TIMEOUT;
extern const char *DEVICE_DEFAULT_NAME; extern const char *DEVICE_DEFAULT_NAME;
extern const char *FIFO_PATH; extern const char *FIFO_PATH;
extern const uint32_t CONNECTION_RETRY_DELAY; extern const struct timespec CONNECTION_RETRY_DELAY;
extern const uint16_t VIRTUAL_DEVICE_VENDOR; extern const uint16_t VIRTUAL_DEVICE_VENDOR;
extern const uint16_t VIRTUAL_DEVICE_PRODUCT; extern const uint16_t VIRTUAL_DEVICE_PRODUCT;
extern const uint16_t VIRTUAL_DEVICE_VERSION; extern const uint16_t VIRTUAL_DEVICE_VERSION;

370
hid.c
View File

@ -1,6 +1,7 @@
#include "hid.h" #include "hid.h"
#include "const.h" #include "const.h"
#include "server.h"
#include "util.h" #include "util.h"
#include "vec.h" #include "vec.h"
@ -18,17 +19,19 @@
#include <unistd.h> #include <unistd.h>
// List of uniq of the currently known devices // List of uniq of the currently known devices
static Vec devices; static Vec known_devices;
// List of the new devices of a poll, static to keep the allocation alive // Queue of available devices, devices that can only be given to one client
static Vec new_devices; static Vec available_devices;
// Queue of devices to be taken by connections // List of cloneable devices, devices that can be handed out to multiple clients
static Vec devices_queue; static Vec cloneable_devices;
// Mutex for the device queue
static pthread_mutex_t devices_queue_mutex = PTHREAD_MUTEX_INITIALIZER;
// Condvar notified on device queue update
static pthread_cond_t devices_queue_cond = PTHREAD_COND_INITIALIZER;
// Mutex for devices // Mutex for devices
static pthread_mutex_t devices_mutex = PTHREAD_MUTEX_INITIALIZER; static pthread_mutex_t devices_mutex = PTHREAD_MUTEX_INITIALIZER;
// Condvar notified on devices update
static pthread_cond_t devices_cond = PTHREAD_COND_INITIALIZER;
// Mutex for devices
static pthread_mutex_t known_devices_mutex = PTHREAD_MUTEX_INITIALIZER;
static ServerConfig *config;
// uniqs are just hexadecimal numbers with colons in between each byte // uniqs are just hexadecimal numbers with colons in between each byte
uniq_t parse_uniq(char uniq[17]) { uniq_t parse_uniq(char uniq[17]) {
@ -64,8 +67,8 @@ void setup_device(PhysicalDevice *dev) {
for (int i = 0; i < KEY_CNT; i++) for (int i = 0; i < KEY_CNT; i++)
dev->mapping.key_indices[i] = -1; dev->mapping.key_indices[i] = -1;
uint8_t type_bits[EV_MAX] = {}; uint8_t type_bits[EV_MAX] = {0};
uint8_t feat_bits[(KEY_MAX + 7) / 8] = {}; uint8_t feat_bits[(KEY_MAX + 7) / 8] = {0};
ioctl(dev->event, EVIOCGBIT(0, EV_MAX), type_bits); ioctl(dev->event, EVIOCGBIT(0, EV_MAX), type_bits);
// Loop over all event types // Loop over all event types
@ -116,13 +119,8 @@ void setup_device(PhysicalDevice *dev) {
} }
} }
// Function used to filter out devices that we don't want. bool filter_event(int fd, char *event, ControllerFilter *filter) {
// This is pretty arbritrary if (filter->js) {
bool filter_event(int fd, char *event) {
// Check for existance of a js* directory in /sys/class/input/eventXX/device
// This is used to filter out the touchpad of PS4 controller (which have the same product and vendor id as
// the controller)
{
char device_path[64]; char device_path[64];
snprintf(device_path, 64, "/sys/class/input/%s/device", event); snprintf(device_path, 64, "/sys/class/input/%s/device", event);
@ -144,85 +142,112 @@ bool filter_event(int fd, char *event) {
} }
} }
// Check product and vendor id 054c:05c4 => Dualshock 4 (09cc is for the second generation) struct input_id ids;
uint16_t info[4]; ioctl(fd, EVIOCGID, &ids);
ioctl(fd, EVIOCGID, info);
return info[1] == 0x054c && (info[2] == 0x05c4 || info[2] == 0x09cc); if (filter->vendor > 0 && filter->vendor != ids.vendor)
return false;
if (filter->product > 0 && filter->product != ids.product)
return false;
return true;
} }
// Initialize vectors for polling // Initialize vectors for polling
void poll_devices_init() { void poll_devices_init() {
devices = vec_of(uniq_t); known_devices = vec_of(Controller);
new_devices = vec_of(PhysicalDevice); cloneable_devices = vec_of(Controller *);
devices_queue = vec_of(PhysicalDevice); available_devices = vec_of(Controller *);
} }
// Block to get a device, this is thread safe // Block to get a device, this is thread safe
PhysicalDevice get_device() { Controller *get_device(char *tag) {
// Check if we can get one right away // Check if we can get one right away
pthread_mutex_lock(&devices_queue_mutex); pthread_mutex_lock(&devices_mutex);
if (devices_queue.len > 0) {
PhysicalDevice r;
vec_pop(&devices_queue, &r);
pthread_mutex_unlock(&devices_queue_mutex);
return r; while (1) {
for (int i = 0; i < available_devices.len; i++) {
Controller *c = *(Controller **)vec_get(&available_devices, i);
if (strcmp(c->ctr.tag, tag) == 0) {
vec_remove(&available_devices, i, NULL);
pthread_mutex_unlock(&devices_mutex);
return c;
}
}
for (int i = 0; i < cloneable_devices.len; i++) {
Controller *c = *(Controller **)vec_get(&cloneable_devices, i);
if (strcmp(c->ctr.tag, tag) == 0) {
pthread_mutex_unlock(&devices_mutex);
return c;
}
}
// Wait on condvar until there's a device and we can unlock the mutex
pthread_cond_wait(&devices_cond, &devices_mutex);
} }
// Wait on condvar until there's a device and we can unlock the mutex
while (devices_queue.len == 0) {
pthread_cond_wait(&devices_queue_cond, &devices_queue_mutex);
}
// Take a device from the queue
PhysicalDevice res;
vec_pop(&devices_queue, &res);
// Signal another thread if there are still device(s) left in the queue
if (devices_queue.len > 0) {
pthread_cond_signal(&devices_queue_cond);
}
pthread_mutex_unlock(&devices_queue_mutex);
return res;
} }
// Forget about a device. This is used on two cases: // Return a device that isn't used anymore, this really only makes sense for non cloneable devices.
// - If the connection to a client is lost, the device is forgotten, picked up by the next device poll, and void return_device(Controller *c) {
// put back in the queue // If device is cloneable there is nothing to return
// - If the device dies (i.e unplugged), the connection to the client is closed and the device forgotten. if (c->ctr.duplicate) {
// return;
// This is thread safe
void return_device(PhysicalDevice *dev) {
if (dev->name != NULL && dev->name != DEVICE_DEFAULT_NAME) {
// Free the name if it was allocated
printf("HID: Returning device '%s' (%012lx)\n", dev->name, dev->uniq);
free(dev->name);
} else {
printf("HID: Returning device %012lx\n", dev->uniq);
} }
pthread_mutex_lock(&devices_mutex);
vec_push(&available_devices, &c);
// Signal that there are new devices
pthread_cond_broadcast(&devices_cond);
pthread_mutex_unlock(&devices_mutex);
}
// Forget about a broken device. This invalidates the reference to the controller
void forget_device(Controller *c) {
// If controller is cloneable we need to remove it from the cloneable list
if (c->ctr.duplicate) {
pthread_mutex_lock(&devices_mutex);
for (int i = 0; i < cloneable_devices.len; i++) {
Controller *d = *(Controller **)vec_get(&cloneable_devices, i);
if (d->dev.uniq == c->dev.uniq) {
vec_remove(&cloneable_devices, i, NULL);
break;
}
}
pthread_mutex_unlock(&devices_mutex);
}
// Free the name if it was allocated
if (c->dev.name != NULL && c->dev.name != DEVICE_DEFAULT_NAME) {
printf("HID: Forgetting device '%s' (%012lx)\n", c->dev.name, c->dev.uniq);
free(c->dev.name);
} else {
printf("HID: Forgetting device %012lx\n", c->dev.uniq);
}
// try to close the file descriptor, they may be already closed if the device was unpugged. // try to close the file descriptor, they may be already closed if the device was unpugged.
close(dev->event); close(c->dev.event);
close(dev->hidraw); close(c->dev.hidraw);
// Safely remove device from the known device list // Safely remove device from the known device list
pthread_mutex_lock(&devices_mutex); pthread_mutex_lock(&known_devices_mutex);
for (int i = 0; i < devices.len; i++) { for (int i = 0; i < known_devices.len; i++) {
uniq_t *uniq = vec_get(&devices, i); Controller *d = vec_get(&known_devices, i);
if (*uniq == dev->uniq) { if (d->dev.uniq == c->dev.uniq) {
vec_remove(&devices, i, NULL); vec_remove(&known_devices, i, NULL);
break; break;
} }
} }
pthread_mutex_unlock(&devices_mutex); pthread_mutex_unlock(&known_devices_mutex);
} }
// Find all available devices and pick up on new ones // Find all available devices and pick up on new ones
void poll_devices() { void poll_devices() {
vec_clear(&new_devices);
// loop over all entries of /sys/class/input // loop over all entries of /sys/class/input
DIR *input_dir = opendir("/sys/class/input"); DIR *input_dir = opendir("/sys/class/input");
struct dirent *input; struct dirent *input;
while ((input = readdir(input_dir)) != NULL) { while ((input = readdir(input_dir)) != NULL) {
// Ignore if the entry isn't a link or doesn't start with event // Ignore if the entry isn't a link 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) {
@ -230,93 +255,117 @@ void poll_devices() {
} }
PhysicalDevice dev; PhysicalDevice dev;
dev.hidraw = -1;
// Open /dev/input/eventXX // Open /dev/input/eventXX
char event_path[64]; {
snprintf(event_path, 64, "/dev/input/%s", input->d_name); char event_path[64];
snprintf(event_path, 64, "/dev/input/%s", input->d_name);
dev.event = open(event_path, O_RDONLY); dev.event = open(event_path, O_RDONLY);
if (dev.event < 0) { // Ignore device if we couldn't open if (dev.event < 0) { // Ignore device if we couldn't open
continue; continue;
}
} }
// Try to get the name, default to DEFAULT_NAME if impossible // Try to get the name, default to DEFAULT_NAME if impossible
char name_buf[256] = {}; char *name;
const char *name; {
if (ioctl(dev.event, EVIOCGNAME(256), name_buf) >= 0) { static char name_buf[256] = {0};
name = name_buf; if (ioctl(dev.event, EVIOCGNAME(256), name_buf) >= 0) {
} else { name = name_buf;
name = DEVICE_DEFAULT_NAME; } else {
name = (char *)DEVICE_DEFAULT_NAME;
}
} }
// Filter events we don't care about // Used for linear searches
if (!filter_event(dev.event, input->d_name)) { bool found;
goto skip;
// Filter devices according server config
ServerConfigController *ctr;
{
found = false;
for (int i = 0; i < config->controller_count; i++) {
ctr = &config->controllers[i];
if (filter_event(dev.event, input->d_name, &ctr->filter)) {
found = true;
break;
}
}
if (!found) {
goto skip;
}
} }
// Try to get uniq, drop device if we can't // Try to get uniq, drop device if we can't
uniq_t uniq;
{ {
char uniq_str[17] = {}; char uniq_str[17] = {0};
ioctl(dev.event, EVIOCGUNIQ(17), uniq_str); ioctl(dev.event, EVIOCGUNIQ(17), uniq_str);
uniq = parse_uniq(uniq_str); dev.uniq = parse_uniq(uniq_str);
// If we couldn't parse the uniq (this assumes uniq can't be zero, which is probably alright) // If we couldn't parse the uniq (this assumes uniq can't be zero, which is probably alright)
if (uniq == 0) { if (dev.uniq == 0) {
goto skip; goto skip;
} }
} }
// Check if we already know of this device // Check if we already know of this device
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) {
found = true;
break;
}
}
pthread_mutex_unlock(&devices_mutex);
if (found) { // Device isn't new
goto skip;
}
dev.uniq = uniq;
// Try to find hidraw path for the device, drop the device if we can't
char hidraw_path[64];
{ {
char hidraw_dir_path[256]; found = false;
snprintf(hidraw_dir_path, 256, "/sys/class/input/%s/device/device/hidraw", input->d_name);
DIR *hidraw_dir = opendir(hidraw_dir_path); pthread_mutex_lock(&known_devices_mutex);
struct dirent *hidraw = NULL; for (int i = 0; i < known_devices.len; i++) {
while ((hidraw = readdir(hidraw_dir)) != NULL) { Controller *c = vec_get(&known_devices, i);
if (strncmp(hidraw->d_name, "hidraw", 6) == 0) { if (c->dev.uniq == dev.uniq) {
found = true;
break; break;
} }
} }
pthread_mutex_unlock(&known_devices_mutex);
if (hidraw == NULL) { if (found) { // Device isn't new
printf("Couldn't get hidraw of %s", input->d_name); goto skip;
continue;
} }
snprintf(hidraw_path, 64, "/dev/%s", hidraw->d_name);
closedir(hidraw_dir);
} }
dev.hidraw = open(hidraw_path, O_WRONLY); // Look for hidraw if the device should have one (Dualshock 4 only, with ps4_hidraw property set)
if (dev.hidraw < 0) { if (ctr->ps4_hidraw) {
goto skip; // Attempt to find the path
char hidraw_path[64];
{
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_dir_path);
struct dirent *hidraw = NULL;
while ((hidraw = readdir(hidraw_dir)) != NULL) {
if (strncmp(hidraw->d_name, "hidraw", 6) == 0) {
break;
}
}
if (hidraw == NULL) {
printf("HID: Couldn't get hidraw of %s", input->d_name);
goto skip;
}
snprintf(hidraw_path, 64, "/dev/%s", hidraw->d_name);
closedir(hidraw_dir);
}
// Try to open
dev.hidraw = open(hidraw_path, O_WRONLY);
if (dev.hidraw < 0) {
goto skip;
}
} }
// Allocate for name (only now to avoid unecessary allocations)
if (name != DEVICE_DEFAULT_NAME) { if (name != DEVICE_DEFAULT_NAME) {
dev.name = malloc(256); dev.name = malloc(256);
@ -327,40 +376,55 @@ void poll_devices() {
} }
} }
setup_device(&dev); // This code is only run if the device has passed all filters and requirements
{
setup_device(&dev);
Controller c = {.dev = dev, .ctr = *ctr};
pthread_mutex_lock(&devices_mutex); pthread_mutex_lock(&known_devices_mutex);
vec_push(&devices, &uniq); // Index of the device in known_devices
pthread_mutex_unlock(&devices_mutex); int index = known_devices.len;
vec_push(&known_devices, &c);
pthread_mutex_unlock(&known_devices_mutex);
vec_push(&new_devices, &dev); // Pointer to the device in known_devices
Controller *p = vec_get(&known_devices, index);
printf("HID: New device, %s (%s: %012lx)\n", name, input->d_name, dev.uniq); printf("HID: New device, %s [%s] (%s: %012lx)\n", name, ctr->tag, input->d_name, dev.uniq);
// Continue here to avoid running cleanup code of skip
if (ctr->duplicate) {
pthread_mutex_lock(&devices_mutex);
vec_push(&cloneable_devices, &p);
// Signal that there are new cloneable devices
pthread_cond_broadcast(&devices_cond);
pthread_mutex_unlock(&devices_mutex);
} else {
pthread_mutex_lock(&devices_mutex);
vec_push(&available_devices, &p);
// Signal that there are new devices
pthread_cond_broadcast(&devices_cond);
pthread_mutex_unlock(&devices_mutex);
}
}
// Continue here avoids running cleanup code
continue; continue;
// close open file descriptor and continue // close open file descriptor and continue
skip: skip:
close(dev.event); close(dev.event);
continue;
}; };
closedir(input_dir); closedir(input_dir);
// Safely add new devices to the queue
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
pthread_cond_signal(&devices_queue_cond);
pthread_mutex_unlock(&devices_queue_mutex);
}
} }
// "Execute" a MessageControllerState: set the led color, rumble and flash using the hidraw interface // "Execute" a MessageControllerState: set the led color, rumble and flash using the hidraw interface (Dualshock 4 only)
void apply_controller_state(PhysicalDevice *dev, MessageControllerState *state) { void apply_controller_state(Controller *c, MessageControllerState *state) {
printf("HID: (%012lx) Controller state: #%02x%02x%02x flash: (%d, %d) rumble: (%d, %d)\n", dev->uniq, if (c->ctr.ps4_hidraw && c->dev.hidraw < 0) {
state->led[0], state->led[1], state->led[2], state->flash_on, state->flash_off, printf("HID: Trying to apply controller state on incompatible device (%012lx)\n", c->dev.uniq);
state->small_rumble, state->big_rumble); return;
}
printf("HID: (%012lx) Controller state: #%02x%02x%02x flash: (%d, %d) rumble: (%d, %d)\n", c->dev.uniq, 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}; uint8_t buf[32] = {0x05, 0xff, 0x00, 0x00};
@ -372,24 +436,24 @@ void apply_controller_state(PhysicalDevice *dev, MessageControllerState *state)
buf[9] = state->flash_on; buf[9] = state->flash_on;
buf[10] = state->flash_off; buf[10] = state->flash_off;
write(dev->hidraw, buf, 32); write(c->dev.hidraw, buf, 32);
if (state->flash_on == 0 && state->flash_off == 0) { if (state->flash_on == 0 && state->flash_off == 0) {
// May not be necessary // May not be necessary
fsync(dev->hidraw); fsync(c->dev.hidraw);
// Send a second time, to reenable the led // Send a second time, to reenable the led
write(dev->hidraw, buf, 32); write(c->dev.hidraw, buf, 32);
}; };
} }
// Body of the hid thread // Body of the hid thread
void *hid_thread() { void *hid_thread(void *arg) {
printf("HID: start\n"); printf("HID: start\n");
config = arg;
poll_devices_init(); poll_devices_init();
while (1) { while (1) {
poll_devices(); poll_devices();
nanosleep(&config->poll_interval, NULL);
nanosleep(&POLL_DEVICE_INTERVAL, NULL);
} }
return NULL; return NULL;

15
hid.h
View File

@ -2,6 +2,7 @@
#ifndef HID_H_ #ifndef HID_H_
#define HID_H_ #define HID_H_
#include "net.h" #include "net.h"
#include "server.h"
#include <linux/input-event-codes.h> #include <linux/input-event-codes.h>
#include <stdint.h> #include <stdint.h>
@ -29,9 +30,15 @@ typedef struct {
MessageDeviceInfo device_info; MessageDeviceInfo device_info;
} PhysicalDevice; } PhysicalDevice;
void *hid_thread(); typedef struct {
void return_device(PhysicalDevice *dev); PhysicalDevice dev;
PhysicalDevice get_device(); ServerConfigController ctr;
void apply_controller_state(PhysicalDevice *dev, MessageControllerState *state); } Controller;
void *hid_thread(void *arg);
void return_device(Controller *c);
void forget_device(Controller *c);
Controller *get_device(char *tag);
void apply_controller_state(Controller *c, MessageControllerState *state);
#endif #endif

223
json.c
View File

@ -22,6 +22,16 @@ JSONError json_errno() { return jerrno; }
// Get the location of the last json parsing error // Get the location of the last json parsing error
size_t json_errloc() { return jerr_index; } size_t json_errloc() { return jerr_index; }
static inline bool is_primitive(const JSONAdapter *adapter) { return adapter->props == NULL; }
static const char *json_type_name(JSONType type) {
if (type > 0 && type < 7) {
return JSONTypeName[type];
} else {
return JSONTypeName[0];
}
}
// Shorthand to set jerno and return -1; // Shorthand to set jerno and return -1;
// i.e // i.e
// ```c // ```c
@ -111,27 +121,27 @@ static inline int json_parse_string(const char **buf, const char *buf_end, uint8
return set_jerrno(DstOverflow); return set_jerrno(DstOverflow);
} }
*(*dst)++ = 0b11000000 | (un_codepoint >> 6 & 0b011111); *(*dst)++ = 0xC0 | (un_codepoint >> 6 & 0x1F);
*(*dst)++ = 0b10000000 | (un_codepoint >> 0 & 0b111111); *(*dst)++ = 0x80 | (un_codepoint >> 0 & 0x3F);
header->len += 2; header->len += 2;
} else if (un_codepoint <= 0xffff) { // 3 byte codepoint } else if (un_codepoint <= 0xffff) { // 3 byte codepoint
if (*dst + 3 >= dst_end) { if (*dst + 3 >= dst_end) {
return set_jerrno(DstOverflow); return set_jerrno(DstOverflow);
} }
*(*dst)++ = 0b11100000 | (un_codepoint >> 12 & 0b1111); *(*dst)++ = 0xE0 | (un_codepoint >> 12 & 0x0F);
*(*dst)++ = 0b10000000 | (un_codepoint >> 6 & 0b111111); *(*dst)++ = 0x80 | (un_codepoint >> 6 & 0x3F);
*(*dst)++ = 0b10000000 | (un_codepoint >> 0 & 0b111111); *(*dst)++ = 0x80 | (un_codepoint >> 0 & 0x3F);
header->len += 3; header->len += 3;
} else if (un_codepoint <= 0x10ffff) { // 4 byte codepoint } else if (un_codepoint <= 0x10ffff) { // 4 byte codepoint
if (*dst + 4 >= dst_end) { if (*dst + 4 >= dst_end) {
return set_jerrno(DstOverflow); return set_jerrno(DstOverflow);
} }
*(*dst)++ = 0b11110000 | (un_codepoint >> 18 & 0b111); *(*dst)++ = 0xF0 | (un_codepoint >> 18 & 0x07);
*(*dst)++ = 0b10000000 | (un_codepoint >> 12 & 0b111111); *(*dst)++ = 0x80 | (un_codepoint >> 12 & 0x3F);
*(*dst)++ = 0b10000000 | (un_codepoint >> 6 & 0b111111); *(*dst)++ = 0x80 | (un_codepoint >> 6 & 0x3F);
*(*dst)++ = 0b10000000 | (un_codepoint >> 0 & 0b111111); *(*dst)++ = 0x80 | (un_codepoint >> 0 & 0x3F);
header->len += 4; header->len += 4;
} else { // Illegal codepoint } else { // Illegal codepoint
return set_jerrno(StringBadUnicode); return set_jerrno(StringBadUnicode);
@ -431,6 +441,7 @@ static int json_parse_array(const char **buf, const char *buf_end, uint8_t **res
} }
if (**buf == ']') { // Array is empty if (**buf == ']') { // Array is empty
(*buf)++;
header->len = 0; header->len = 0;
return 0; return 0;
} }
@ -490,6 +501,7 @@ static int json_parse_object(const char **buf, const char *buf_end, uint8_t **re
return set_jerrno(SrcOverflow); return set_jerrno(SrcOverflow);
} }
if (**buf == '}') { if (**buf == '}') {
(*buf)++;
// The object is empty // The object is empty
header->len = 0; header->len = 0;
return 0; return 0;
@ -661,49 +673,186 @@ void json_print_value_priv(uint8_t **buf) {
// /!\ doesn't handle strings well // /!\ doesn't handle strings well
void json_print_value(uint8_t *buf) { json_print_value_priv(&buf); } void json_print_value(uint8_t *buf) { json_print_value_priv(&buf); }
// Loop over adapters and set accordingly void json_print_buffer(uint8_t *buf) {
static void json_adapt_set(uint8_t *buf, JSONAdapter *adapters, size_t adapter_count, void *ptr, char *path) { uint8_t *end = buf + align_8(((JSONHeader *)buf)->len) + sizeof(JSONHeader);
JSONHeader *header = (JSONHeader *)buf; while (buf < end) {
JSONHeader *header = (JSONHeader *)buf;
printf("[\033[32m%s\033[0m][\033[31m%lu\033[0m]", json_type_name(header->type), align_8(header->len));
buf += sizeof(JSONHeader);
for (int i = 0; i < adapter_count; i++) { if (header->type == Object || header->type == Array)
if (strcmp(path, adapters[i].path) == 0 && header->type == adapters[i].type) { continue;
void *p = ptr + adapters[i].offset; printf("[\033[34m");
switch (header->type) {
case Number:
printf("%lf", *(double *)buf);
break;
case Boolean:
printf("%s", *(uint64_t *)buf == 1 ? "true" : "false");
break;
case Null:
printf("null");
break;
case String:
printf("\"%.*s\"", header->len, (char *)buf);
break;
}
printf("\033[0m]");
switch (header->type) { buf += align_8(header->len);
case String: { }
char *v = malloc(header->len + 1); printf("\n");
strncpy(v, (char *)(buf + sizeof(JSONHeader)), header->len); }
v[header->len] = '\0';
*(char **)p = v; static inline bool ends_with(const char *str, const char *pat) {
} break; size_t strl = strlen(str);
case Number: size_t patl = strlen(pat);
*(double *)p = *(double *)(buf + sizeof(JSONHeader)); return strl >= patl && strcmp(str + strl - patl, pat) == 0;
break; }
case Boolean:
*(bool *)p = *(uint64_t *)(buf + sizeof(JSONHeader)) == 1; static void json_adapt_set_defaults(const JSONAdapter *adapter, void *ptr) {
break; if (!is_primitive(adapter)) {
for (int i = 0; i < adapter->prop_count; i++) {
uint8_t *p = (uint8_t*)ptr + adapter->props[i].offset;
if (ends_with(adapter->props[i].path, "[]")) {
*(size_t *)(p + sizeof(void *)) = 0;
}
if (adapter->props[i].default_func != NULL) {
adapter->props[i].default_func(p);
} else if (!is_primitive(adapter->props[i].type)) {
json_adapt_set_defaults(adapter->props[i].type, p);
} }
} }
} }
} }
// Run adapters on a value // Run adapters on a value
static void json_adapt_priv(uint8_t **buf, JSONAdapter *adapters, size_t adapter_count, void *ptr, // buf: is a reference of the pointer to the json buffer
// adapter: is the adapter to run
// ptr: points where to write the data
// path_buffer: points to the begining of the path buffer
// full_path: points to the "current" path
// path: points to the end of the current path (most of the times)
static void json_adapt_priv(uint8_t **buf, const JSONAdapter *adapter, void *ptr, char *path_buffer,
char *full_path, char *path) { char *full_path, char *path) {
JSONHeader *header = (JSONHeader *)*buf; JSONHeader *header = (JSONHeader *)*buf;
if (is_primitive(adapter)) {
// The type of a primitive adapter is stored in prop_count
JSONType type = adapter->prop_count;
if (type != header->type) {
printf("JSON: Mismatched type on %s: expected %s got %s\n", path_buffer, json_type_name(type),
json_type_name(header->type));
return;
}
*buf += sizeof(JSONHeader);
if (type == Boolean) {
*(bool *)ptr = *(uint64_t *)(*buf) == 1;
} else if (type == Number) {
*(double *)ptr = *(double *)(*buf);
} else if (type == String) {
char *v = malloc(header->len + 1);
strncpy(v, (char *)(*buf), header->len);
v[header->len] = '\0';
*(char **)ptr = v;
} else {
printf("JSON: Unknown or illegal primitive adapter of type %s\n", json_type_name(type));
}
return;
}
// This is only true once, so we set default values there
if (path == full_path) {
json_adapt_set_defaults(adapter, ptr);
}
if (header->type == Array) {
path[0] = '[';
path[1] = ']';
path[2] = '\0';
}
uint8_t buffer_small[64];
for (int i = 0; i < adapter->prop_count; i++) {
if (strcmp(adapter->props[i].path, full_path) == 0) {
uint8_t *p = (uint8_t*)ptr + adapter->props[i].offset;
size_t size = adapter->props[i].type->size;
if (header->type == Array) {
uint8_t *array_buf = *buf + sizeof(JSONHeader);
uint8_t *end = array_buf + header->len;
size_t len;
for (len = 0; array_buf < end; len++) {
array_buf += align_8(((JSONHeader *)array_buf)->len);
array_buf += sizeof(JSONHeader);
};
uint8_t *array_ptr = malloc(len * size);
array_buf = *buf + sizeof(JSONHeader);
for (size_t index = 0; index < len; index++) {
path[0] = '.';
path[1] = '\0';
json_adapt_priv(&array_buf, adapter->props[i].type, array_ptr + index * size, path_buffer,
path, path);
path[0] = '\0';
}
if (adapter->props[i].transformer != NULL) {
printf("JSON: Transformers aren't yet allowed on arrays\n");
}
*(void **)p = array_ptr;
*(size_t *)(p + sizeof(void *)) = len;
} else {
void *tmp_ptr;
if (size <= 64) {
tmp_ptr = buffer_small;
} else {
tmp_ptr = malloc(size);
}
uint8_t *new_buf = *buf;
path[0] = '.';
path[1] = '\0';
json_adapt_priv(&new_buf, adapter->props[i].type, tmp_ptr, path_buffer, path, path);
path[0] = '\0';
if (adapter->props[i].transformer != NULL) {
adapter->props[i].transformer(tmp_ptr, p);
} else {
memcpy(p, tmp_ptr, size);
}
if (tmp_ptr != buffer_small) {
free(tmp_ptr);
}
}
}
}
switch (header->type) { switch (header->type) {
case String: case String:
json_adapt_set(*buf, adapters, adapter_count, ptr, full_path);
*buf += sizeof(JSONHeader) + align_8(header->len); *buf += sizeof(JSONHeader) + align_8(header->len);
break; break;
case Number: case Number:
json_adapt_set(*buf, adapters, adapter_count, ptr, full_path);
*buf += sizeof(JSONHeader) + sizeof(double); *buf += sizeof(JSONHeader) + sizeof(double);
break; break;
case Boolean: case Boolean:
json_adapt_set(*buf, adapters, adapter_count, ptr, full_path);
*buf += sizeof(JSONHeader) + 8; *buf += sizeof(JSONHeader) + 8;
break; break;
case Null: case Null:
@ -714,7 +863,7 @@ static void json_adapt_priv(uint8_t **buf, JSONAdapter *adapters, size_t adapter
uint8_t *end = *buf + header->len; uint8_t *end = *buf + header->len;
for (size_t index = 0; *buf < end; index++) { for (size_t index = 0; *buf < end; index++) {
int len = sprintf(path, ".%lu", index); int len = sprintf(path, ".%lu", index);
json_adapt_priv(buf, adapters, adapter_count, ptr, full_path, path + len); json_adapt_priv(buf, adapter, ptr, path_buffer, full_path, path + len);
} }
} break; } break;
case Object: { case Object: {
@ -727,14 +876,14 @@ static void json_adapt_priv(uint8_t **buf, JSONAdapter *adapters, size_t adapter
int len = sprintf(path, ".%.*s", key_header->len, *buf); int len = sprintf(path, ".%.*s", key_header->len, *buf);
*buf += align_8(key_header->len); *buf += align_8(key_header->len);
json_adapt_priv(buf, adapters, adapter_count, ptr, full_path, path + len); json_adapt_priv(buf, adapter, ptr, path_buffer, full_path, path + len);
} }
} break; } break;
} }
} }
// Run adapters on a json value // Run adapter on a json value
void json_adapt(uint8_t *buf, JSONAdapter *adapters, size_t adapter_count, void *ptr) { void json_adapt(uint8_t *buf, const JSONAdapter *adapter, void *ptr) {
char path[512] = "."; char path[512] = ".";
json_adapt_priv(&buf, adapters, adapter_count, ptr, path, path); json_adapt_priv(&buf, adapter, ptr, path, path, path);
} }

71
json.h
View File

@ -4,6 +4,7 @@
#include <limits.h> #include <limits.h>
#include <stddef.h> #include <stddef.h>
#include <stdint.h> #include <stdint.h>
#include <stdbool.h>
typedef struct __attribute__((packed, aligned(8))) { typedef struct __attribute__((packed, aligned(8))) {
uint32_t type; uint32_t type;
@ -37,6 +38,36 @@ typedef enum {
JERRORNO_MAX = 10 JERRORNO_MAX = 10
} JSONError; } JSONError;
struct JSONAdapter;
typedef struct {
char *path;
const struct JSONAdapter *type;
size_t offset;
// Function setting default value
void (*default_func)(void *ptr);
// Optional transformer, can be NULL
void (*transformer)(void *arg, void *ptr);
} JSONPropertyAdapter;
struct JSONAdapter {
const JSONPropertyAdapter *props;
size_t size;
size_t prop_count;
};
typedef struct JSONAdapter JSONAdapter;
void json_adapt(uint8_t *buf, const JSONAdapter *adapter, void *ptr);
int json_parse(const char *src, size_t src_len, uint8_t *dst, size_t dst_len);
void json_print_value(uint8_t *buf);
const char *json_strerr();
size_t json_errloc();
JSONError json_errno();
extern const JSONAdapter NumberAdapter;
extern const JSONAdapter StringAdapter;
extern const JSONAdapter BooleanAdapter;
#ifdef JSON_C_ #ifdef JSON_C_
static const char *JSONErrorMessage[JERRORNO_MAX + 1] = { static const char *JSONErrorMessage[JERRORNO_MAX + 1] = {
"No error", "No error",
@ -51,20 +82,32 @@ static const char *JSONErrorMessage[JERRORNO_MAX + 1] = {
"Unexpected character in object", "Unexpected character in object",
"?", "?",
}; };
static const char * JSONTypeName[7] = {
"[Unknown]",
"String",
"Number",
"Object",
"Array",
"Boolean",
"Null",
};
const JSONAdapter NumberAdapter = {
.prop_count = Number,
.props = NULL,
.size = sizeof(double),
};
const JSONAdapter StringAdapter = {
.prop_count = String,
.props = NULL,
.size = sizeof(char *),
};
const JSONAdapter BooleanAdapter = {
.prop_count = Boolean,
.props = NULL,
.size = sizeof(bool),
};
#endif #endif
// See client.c for usage of adapters
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);
void json_print_value(uint8_t *buf);
const char *json_strerr();
size_t json_errloc();
JSONError json_errno();
#endif #endif

36
main.c
View File

@ -3,30 +3,26 @@
#include "server.h" #include "server.h"
#include "util.h" #include "util.h"
#include <pthread.h>
#include <stdint.h> #include <stdint.h>
#include <stdio.h> #include <stdio.h>
#include <string.h> #include <string.h>
const char *USAGE[] = { const char *USAGE[] = {
"jsfw client [address] [port]\n", "jsfw client [address] [port] [config]\n",
"jsfw server [port]\n", "jsfw server [port] [config]\n",
}; };
// Start the server // Start the server
void server(uint16_t port) { void server(uint16_t port, char *config_path) {
printf("[Server (0.0.0.0:%u)]\n\n", port); printf("[Server (0.0.0.0:%u)] <- %s\n\n", port, config_path);
pthread_t thread; server_run(port, config_path);
pthread_create(&thread, NULL, hid_thread, NULL);
server_run(port);
} }
// Start the client // Start the client
void client(char *address, uint16_t port) { void client(char *address, uint16_t port, char *config_path) {
printf("[Client (%s:%d)]\n\n", address, port); printf("[Client (%s:%d)] <- %s\n\n", address, port, config_path);
client_run(address, port); client_run(address, port, config_path);
} }
int main(int argc, char *argv[]) { int main(int argc, char *argv[]) {
@ -39,21 +35,23 @@ int main(int argc, char *argv[]) {
char *mode = argv[1]; char *mode = argv[1];
if (strcmp(mode, "server") == 0) { if (strcmp(mode, "server") == 0) {
if (argc < 3) { if (argc < 4) {
panicf("Usage: %s", USAGE[1]); panicf("Usage: %s", USAGE[1]);
} }
uint16_t port = parse_port(argv[2]); uint16_t port = parse_port(argv[2]);
server(port); char *config_path = argv[3];
server(port, config_path);
} else if (strcmp(mode, "client") == 0) { } else if (strcmp(mode, "client") == 0) {
if (argc < 4) { if (argc < 5) {
panicf("Usage: %s", USAGE[0]); panicf("Usage: %s", USAGE[0]);
} }
char *address = argv[2]; char *address = argv[2];
uint16_t port = parse_port(argv[3]); uint16_t port = parse_port(argv[3]);
client(address, port); char *config_path = argv[4];
client(address, port, config_path);
} else { } else {
printf("Unknown mode: '%s'\n", mode); printf("Unknown mode: '%s'\n", mode);

168
net.c
View File

@ -1,6 +1,9 @@
#include "net.h" #include "net.h"
#include "util.h"
#include <stdio.h> #include <stdio.h>
#include <string.h>
// Deserialize the message in buf, buf must be at least 4 aligned. Returns -1 on error, otherwise returns 0 // Deserialize the message in buf, buf must be at least 4 aligned. Returns -1 on error, otherwise returns 0
// and writes result to dst // and writes result to dst
@ -12,7 +15,7 @@ int msg_deserialize(const uint8_t *buf, size_t len, Message *restrict dst) {
uint8_t code_byte = buf[0]; uint8_t code_byte = buf[0];
MessageCode code = (MessageCode)code_byte; MessageCode code = (MessageCode)code_byte;
uint16_t abs, rel, key, *buf16; uint16_t abs, rel, key, index, *buf16;
switch (code) { switch (code) {
case DeviceInfo: case DeviceInfo:
@ -20,14 +23,16 @@ int msg_deserialize(const uint8_t *buf, size_t len, Message *restrict dst) {
return -1; return -1;
// buf + 2: a byte for code and a byte for padding // buf + 2: a byte for code and a byte for padding
buf16 = (uint16_t *)(buf + 2); buf16 = (uint16_t *)(buf + 2);
abs = buf16[0]; index = buf16[0];
rel = buf16[1]; abs = buf16[1];
key = buf16[2]; rel = buf16[2];
buf += 8; key = buf16[3];
buf += 12;
if (MSS_DEVICE_INFO(abs, rel, key) > len) if (MSS_DEVICE_INFO(abs, rel, key) > len)
return -1; return -1;
dst->device_info.code = code; dst->device_info.code = code;
dst->device_info.index = index;
dst->device_info.abs_count = abs; dst->device_info.abs_count = abs;
dst->device_info.rel_count = rel; dst->device_info.rel_count = rel;
dst->device_info.key_count = key; dst->device_info.key_count = key;
@ -64,14 +69,16 @@ int msg_deserialize(const uint8_t *buf, size_t len, Message *restrict dst) {
// buf + 2: a byte for code and a byte of padding // buf + 2: a byte for code and a byte of padding
buf16 = (uint16_t *)(buf + 2); buf16 = (uint16_t *)(buf + 2);
abs = buf16[0]; index = buf16[0];
rel = buf16[1]; abs = buf16[1];
key = buf16[2]; rel = buf16[2];
buf += 8; key = buf16[3];
buf += 12;
if (len < MSS_DEVICE_REPORT(abs, rel, key)) if (len < MSS_DEVICE_REPORT(abs, rel, key))
return -1; return -1;
dst->device_report.code = code; dst->device_report.code = code;
dst->device_report.index = index;
dst->device_report.abs_count = abs; dst->device_report.abs_count = abs;
dst->device_report.rel_count = rel; dst->device_report.rel_count = rel;
dst->device_report.key_count = key; dst->device_report.key_count = key;
@ -95,13 +102,59 @@ int msg_deserialize(const uint8_t *buf, size_t len, Message *restrict dst) {
return -1; return -1;
dst->code = code; dst->code = code;
dst->controller_state.led[0] = buf[1]; dst->controller_state.index = *(uint16_t *)(buf + 2);
dst->controller_state.led[1] = buf[2]; dst->controller_state.led[0] = buf[4];
dst->controller_state.led[2] = buf[3]; dst->controller_state.led[1] = buf[5];
dst->controller_state.small_rumble = buf[4]; dst->controller_state.led[2] = buf[6];
dst->controller_state.big_rumble = buf[5]; dst->controller_state.small_rumble = buf[7];
dst->controller_state.flash_on = buf[6]; dst->controller_state.big_rumble = buf[8];
dst->controller_state.flash_off = buf[7]; dst->controller_state.flash_on = buf[9];
dst->controller_state.flash_off = buf[10];
return 0;
case Request: {
if (len < 3)
return -1;
dst->code = code;
dst->request.request_count = *(uint16_t *)(buf + 2);
buf += 4; // 1 bytes for code, 1 byte for padding and 2 bytes for count
int count = dst->request.request_count;
char **tags = malloc(count * sizeof(char *));
// The length of the message, will be updated as we read more.
int expected_len = 3;
for (int i = 0; i < dst->request.request_count; i++) {
expected_len += 2;
if (len < expected_len) {
return -1;
}
uint16_t str_len = *(uint16_t *)buf;
buf += 2;
expected_len += align_2(str_len);
if (len < expected_len) {
return -1;
}
char *str = malloc(str_len + 1);
str[len] = '\0';
strncpy(str, (char *)buf, str_len);
buf += align_2(str_len);
}
dst->request.requests = tags;
return 0;
}
case DeviceDestroy:
if (len < MSS_DESTROY)
return -1;
dst->code = code;
dst->destroy.index = *(uint16_t *)(buf + 2);
return 0; return 0;
default: default:
return -1; return -1;
@ -129,10 +182,11 @@ int msg_serialize(uint8_t *restrict buf, size_t len, const Message *msg) {
// buf + 2: a byte for code and a byte for padding // buf + 2: a byte for code and a byte for padding
buf16 = (uint16_t *)(buf + 2); buf16 = (uint16_t *)(buf + 2);
// 2 aligned here // 2 aligned here
buf16[0] = abs; buf16[0] = msg->device_info.index;
buf16[1] = rel; buf16[1] = abs;
buf16[2] = key; buf16[2] = rel;
buf += 8; buf16[3] = key;
buf += 12;
// Back to 4 aligned // Back to 4 aligned
for (int i = 0; i < abs; i++) { for (int i = 0; i < abs; i++) {
@ -171,10 +225,11 @@ int msg_serialize(uint8_t *restrict buf, size_t len, const Message *msg) {
buf[0] = (uint8_t)msg->code; buf[0] = (uint8_t)msg->code;
// buf + 2: a byte for code and a byte for padding // buf + 2: a byte for code and a byte for padding
buf16 = (uint16_t *)(buf + 2); buf16 = (uint16_t *)(buf + 2);
buf16[0] = abs; buf16[0] = msg->device_report.index;
buf16[1] = rel; buf16[1] = abs;
buf16[2] = key; buf16[2] = rel;
buf += 8; buf16[3] = key;
buf += 12;
// We're 4 aligned already // We're 4 aligned already
for (int i = 0; i < abs; i++) { for (int i = 0; i < abs; i++) {
*(uint32_t *)buf = msg->device_report.abs[i]; *(uint32_t *)buf = msg->device_report.abs[i];
@ -195,16 +250,67 @@ int msg_serialize(uint8_t *restrict buf, size_t len, const Message *msg) {
return -1; return -1;
buf[0] = (uint8_t)msg->code; buf[0] = (uint8_t)msg->code;
buf[1] = msg->controller_state.led[0];
buf[2] = msg->controller_state.led[1]; *(uint16_t *)(buf + 2) = msg->controller_state.index;
buf[3] = msg->controller_state.led[2];
buf[4] = msg->controller_state.small_rumble; buf[4] = msg->controller_state.led[0];
buf[5] = msg->controller_state.big_rumble; buf[5] = msg->controller_state.led[1];
buf[6] = msg->controller_state.flash_on; buf[6] = msg->controller_state.led[2];
buf[7] = msg->controller_state.flash_off; buf[7] = msg->controller_state.small_rumble;
buf[8] = msg->controller_state.big_rumble;
buf[9] = msg->controller_state.flash_on;
buf[10] = msg->controller_state.flash_off;
return MSS_CONTROLLER_STATE + 1; return MSS_CONTROLLER_STATE + 1;
case Request: {
int expected_len = MSS_REQUEST(msg->request.request_count);
if (len < expected_len)
return -1;
buf[0] = (uint8_t)msg->code;
buf16 = (uint16_t *)(buf + 2);
buf16[0] = msg->request.request_count;
buf += 4;
buf16++;
for (int i = 0; i < msg->request.request_count; i++) {
int str_len = strlen(msg->request.requests[i]);
int byte_len = align_2(str_len);
*buf16++ = str_len;
buf = (uint8_t *)buf16;
expected_len += byte_len;
if (len < expected_len) {
return -1;
}
strncpy((char *)buf, msg->request.requests[i], str_len);
buf += byte_len;
// Buf has to be aligned here since byte_len is two aligned and we started off two aligned
buf16 = (uint16_t *)buf;
}
return expected_len;
}
case DeviceDestroy:
if (len < MSS_DESTROY)
return -1;
buf[0] = (uint8_t)msg->code;
*(uint16_t *)(buf + 2) = msg->controller_state.index;
return MSS_DESTROY + 1;
default: default:
printf("ERR(msg_serialize): Trying to serialize unknown message of code %d\n", msg->code); printf("ERR(msg_serialize): Trying to serialize unknown message of code %d\n", msg->code);
return -1; return -1;
} }
} }
void msg_free(Message *msg) {
if (msg->code == Request) {
for (int i = 0; i < msg->request.request_count; i++) {
free(msg->request.requests[i]);
}
free(msg->request.requests);
}
}

46
net.h
View File

@ -6,10 +6,12 @@
#include <stdlib.h> #include <stdlib.h>
typedef enum { typedef enum {
NoMessage = 0,
DeviceInfo = 1, DeviceInfo = 1,
DeviceReport = 2, DeviceReport = 2,
DeviceDestroy = 3, DeviceDestroy = 3,
ControllerState = 4, ControllerState = 4,
Request = 5,
} MessageCode; } MessageCode;
// Alignment 4 // Alignment 4
@ -17,6 +19,8 @@ typedef struct {
MessageCode code; MessageCode code;
// + 1 byte of padding // + 1 byte of padding
uint16_t index;
uint16_t abs_count; uint16_t abs_count;
uint16_t rel_count; uint16_t rel_count;
uint16_t key_count; uint16_t key_count;
@ -33,7 +37,7 @@ typedef struct {
uint16_t key_id[KEY_CNT]; uint16_t key_id[KEY_CNT];
} MessageDeviceInfo; } MessageDeviceInfo;
#define MSS_DEVICE_INFO(abs, rel, key) (8 + abs * 24 + rel * 2 + key * 2 + 1) #define MSS_DEVICE_INFO(abs, rel, key) (10 + abs * 24 + rel * 2 + key * 2 + 1)
// MSS -> Message Serialized Size: // MSS -> Message Serialized Size:
// Size of the data of the message when serialized (no alignment / padding) // Size of the data of the message when serialized (no alignment / padding)
@ -41,6 +45,7 @@ typedef struct {
typedef struct { typedef struct {
MessageCode code; MessageCode code;
// + 1 byte of padding // + 1 byte of padding
uint16_t index;
uint16_t abs_count; uint16_t abs_count;
uint16_t rel_count; uint16_t rel_count;
@ -50,28 +55,49 @@ typedef struct {
uint32_t rel[REL_CNT]; uint32_t rel[REL_CNT];
uint8_t key[KEY_CNT]; uint8_t key[KEY_CNT];
} MessageDeviceReport; } MessageDeviceReport;
#define MSS_DEVICE_REPORT(abs, rel, key) (6 + abs * 4 + rel * 4 + key * 1 + 1) #define MSS_DEVICE_REPORT(abs, rel, key) (8 + abs * 4 + rel * 4 + key * 1 + 1)
// 1 aligned // 1 aligned
typedef struct { typedef struct {
MessageCode code; MessageCode code;
// + 1 byte of padding
uint8_t led[3]; uint16_t index;
uint8_t small_rumble; uint8_t led[3];
uint8_t big_rumble; uint8_t small_rumble;
uint8_t flash_on; uint8_t big_rumble;
uint8_t flash_off; uint8_t flash_on;
uint8_t flash_off;
} MessageControllerState; } MessageControllerState;
#define MSS_CONTROLLER_STATE 7 #define MSS_CONTROLLER_STATE 10
typedef struct {
MessageCode code;
char **requests;
uint16_t request_count;
} MessageRequest;
#define MSS_REQUEST(count) (2 + 2 * count)
typedef struct {
MessageCode code;
// + 1 byte of padding
uint16_t index;
} MessageDestroy;
#define MSS_DESTROY 3
typedef union { typedef union {
MessageCode code; MessageCode code;
MessageRequest request;
MessageDestroy destroy;
MessageDeviceInfo device_info; MessageDeviceInfo device_info;
MessageDeviceReport device_report; MessageDeviceReport device_report;
MessageControllerState controller_state; MessageControllerState controller_state;
} Message; } Message;
int msg_deserialize(const uint8_t *buf, size_t len, Message *restrict dst); 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); int msg_serialize(uint8_t *restrict buf, size_t len, const Message *msg);
void msg_free(Message *msg);
#endif #endif

424
server.c
View File

@ -1,14 +1,20 @@
#include "server.h"
#include "const.h" #include "const.h"
#include "hid.h" #include "hid.h"
#include "json.h"
#include "net.h" #include "net.h"
#include "util.h" #include "util.h"
#include "vec.h"
#include <linux/input-event-codes.h> #include <linux/input-event-codes.h>
#include <linux/input.h> #include <linux/input.h>
#include <math.h>
#include <netinet/in.h> #include <netinet/in.h>
#include <netinet/tcp.h> #include <netinet/tcp.h>
#include <poll.h> #include <poll.h>
#include <pthread.h> #include <pthread.h>
#include <signal.h>
#include <stdint.h> #include <stdint.h>
#include <stdio.h> #include <stdio.h>
#include <stdlib.h> #include <stdlib.h>
@ -22,6 +28,181 @@ struct Connection {
uint32_t id; uint32_t id;
}; };
struct DeviceThreadArgs {
int index;
char *tag;
Controller **controller;
struct Connection *conn;
};
static void default_timespec(void *ptr) { *(struct timespec *)ptr = POLL_DEVICE_INTERVAL; }
static void default_request_timeout(void *ptr) { *(uint32_t *)ptr = REQUEST_TIMEOUT; }
const JSONPropertyAdapter FilterAdapterProps[] = {
{".mac_address", &StringAdapter, offsetof(ControllerFilter, mac_address), default_to_zero_u64, tsf_strmac_to_u64},
{".vendor", &StringAdapter, offsetof(ControllerFilter, vendor), default_to_negative_one_i32, tsf_hex_to_i32 },
{".product", &StringAdapter, offsetof(ControllerFilter, product), default_to_negative_one_i32, tsf_hex_to_i32 },
{".js", &BooleanAdapter, offsetof(ControllerFilter, js), default_to_false, NULL },
};
const JSONAdapter FilterAdapter = {
.props = FilterAdapterProps,
.prop_count = sizeof(FilterAdapterProps) / sizeof(JSONPropertyAdapter),
.size = sizeof(ControllerFilter),
};
const JSONPropertyAdapter ControllerAdapterProps[] = {
{".filter", &FilterAdapter, offsetof(ServerConfigController, filter), NULL, NULL},
{".tag", &StringAdapter, offsetof(ServerConfigController, tag), default_to_null, NULL},
{".properties.duplicate", &BooleanAdapter, offsetof(ServerConfigController, duplicate), default_to_false, NULL},
{".properties.ps4_hidraw", &BooleanAdapter, offsetof(ServerConfigController, ps4_hidraw), default_to_false, NULL},
};
const JSONAdapter ControllerAdapter = {
.props = ControllerAdapterProps,
.prop_count = sizeof(ControllerAdapterProps) / sizeof(JSONPropertyAdapter),
.size = sizeof(ServerConfigController),
};
const JSONPropertyAdapter ConfigAdapterProps[] = {
{".controllers[]", &ControllerAdapter, offsetof(ServerConfig, controllers), default_to_null, NULL },
{".poll_interval", &NumberAdapter, offsetof(ServerConfig, poll_interval), default_timespec, tsf_numsec_to_timespec},
{".request_timeout", &NumberAdapter, offsetof(ServerConfig, request_timeout), default_request_timeout, tsf_numsec_to_intms }
};
const JSONAdapter ConfigAdapter = {
.props = ConfigAdapterProps,
.prop_count = sizeof(ConfigAdapterProps) / sizeof(JSONPropertyAdapter),
.size = sizeof(ServerConfig),
};
static ServerConfig config;
static pthread_key_t device_args_key;
static sigset_t empty_sigset;
#define TRAP(sig, handler) \
if (sigaction(sig, &(struct sigaction){.sa_handler = handler, .sa_mask = empty_sigset, .sa_flags = 0}, NULL) != 0) \
printf("SERVER: can't trap " #sig ".\n")
void device_thread_exit() {
struct DeviceThreadArgs *args = pthread_getspecific(device_args_key);
Controller * ctr = *args->controller;
if(ctr != NULL) {
return_device(ctr);
}
free(args->tag);
free(args);
pthread_exit(NULL);
}
void *device_thread(void *args_) {
struct DeviceThreadArgs *args = args_;
pthread_setspecific(device_args_key, args);
TRAP(SIGTERM, device_thread_exit);
uint8_t buf[2048] __attribute__((aligned(4))) = {0};
MessageDeviceInfo dev_info;
while (true) {
*args->controller = NULL;
Controller *ctr = get_device(args->tag);
*args->controller = ctr;
dev_info = ctr->dev.device_info;
dev_info.index = args->index;
printf("CONN(%d): [%d] Found suitable [%s] device: '%s' (%012lx)\n", args->conn->id, args->index, args->tag,
ctr->dev.name, ctr->dev.uniq);
// Send over device info
{
int len = msg_serialize(buf, 2048, (Message *)&dev_info);
if (write(args->conn->socket, buf, len) == -1) {
printf("CONN(%d): [%d] Couldn't send device info\n", args->conn->id, args->index);
break;
}
}
MessageDeviceReport report = {0};
report.code = DeviceReport;
report.abs_count = ctr->dev.device_info.abs_count;
report.rel_count = ctr->dev.device_info.rel_count;
report.key_count = ctr->dev.device_info.key_count;
while (true) {
struct input_event event;
int len = read(ctr->dev.event, &event, sizeof(struct input_event));
if (len <= 0) {
// We lost the device, so we mark it as broken (we forget it) and try to get a new one (in the next iteration of
// the outer while)
forget_device(ctr);
break;
}
if (len < sizeof(struct input_event)) {
printf("CONN(%d): [%d] error reading event\n", args->conn->id, args->index);
continue;
}
if (event.type == EV_SYN) {
int len = msg_serialize(buf, 2048, (Message *)&report);
if (len < 0) {
printf("CONN(%d): [%d] Couldn't serialize report %d\n", args->conn->id, args->index, len);
continue;
};
send(args->conn->socket, buf, len, 0);
} else if (event.type == EV_ABS) {
int index = ctr->dev.mapping.abs_indices[event.code];
if (index < 0) {
printf("CONN(%d): [%d] Invalid abs\n", args->conn->id, args->index);
continue;
};
report.abs[index] = event.value;
} else if (event.type == EV_REL) {
int index = ctr->dev.mapping.rel_indices[event.code];
if (index < 0) {
printf("CONN(%d): [%d] Invalid rel\n", args->conn->id, args->index);
continue;
};
report.rel[index] = event.value;
} else if (event.type == EV_KEY) {
int index = ctr->dev.mapping.key_indices[event.code];
if (index < 0) {
printf("CONN(%d): [%d] Invalid key\n", args->conn->id, args->index);
continue;
};
report.key[index] = !!event.value;
}
}
// Send device destroy message
{
MessageDestroy dstr;
dstr.code = DeviceDestroy;
dstr.index = args->index;
int len = msg_serialize(buf, 2048, (Message *)&dstr);
if (write(args->conn->socket, buf, len) == -1) {
printf("CONN(%d): [%d] Couldn't send device destroy message\n", args->conn->id, args->index);
break;
}
}
}
device_thread_exit();
return NULL;
}
void *server_handle_conn(void *args_) { void *server_handle_conn(void *args_) {
struct Connection *args = args_; struct Connection *args = args_;
@ -36,154 +217,175 @@ void *server_handle_conn(void *args_) {
if (setsockopt(args->socket, SOL_TCP, TCP_KEEPINTVL, &TCP_KEEPALIVE_RETRY_INTERVAL, sizeof(int)) != 0) if (setsockopt(args->socket, SOL_TCP, TCP_KEEPINTVL, &TCP_KEEPALIVE_RETRY_INTERVAL, sizeof(int)) != 0)
printf("ERR(server_handle_conn): Setting idle retry interval\n"); printf("ERR(server_handle_conn): Setting idle retry interval\n");
uint8_t buf[2048] __attribute__((aligned(4))) = {}; uint8_t buf[2048] __attribute__((aligned(4))) = {0};
PhysicalDevice dev = get_device(); char *closing_message = "";
printf("CONN(%u): got device '%s'\n", args->id, dev.name); bool got_request = false;
Vec device_threads = vec_of(pthread_t);
Vec device_controllers = vec_of(Controller *);
char *closing_message = ""; struct pollfd pfd = {.fd = args->socket, .events = POLLIN};
int len = msg_serialize(buf, 2048, (Message *)&dev.device_info);
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];
struct pollfd *event_poll = &pfds[1];
socket_poll->fd = args->socket;
socket_poll->events = POLLIN;
event_poll->fd = dev.event;
event_poll->events = POLLIN;
MessageDeviceReport report = {};
report.code = DeviceReport;
report.abs_count = dev.device_info.abs_count;
report.rel_count = dev.device_info.rel_count;
report.key_count = dev.device_info.key_count;
while (1) { while (1) {
int rc = poll(pfds, 2, -1); int rc = poll(&pfd, 1, config.request_timeout);
if (rc < 0) { // error (connection closed)
// If poll timed out
if (rc == 0) {
if (!got_request) {
printf("CONN(%d): Didn't get a device request within %i.%03ds\n", args->id, config.request_timeout / 1000,
config.request_timeout % 1000);
closing_message = "Timed out";
goto conn_end;
}
continue;
} else if (rc < 0) { // If poll failed (basically never)
closing_message = "Poll error"; closing_message = "Poll error";
goto conn_end; goto conn_end;
} }
// Shutdown connection if we lost the peer // Test for error on socket
if (socket_poll->revents & POLLHUP || socket_poll->revents & POLLERR) { if (pfd.revents & POLLHUP || pfd.revents & POLLERR) {
closing_message = "Lost peer"; closing_message = "Lost peer";
goto conn_end; goto conn_end;
} }
if (socket_poll->revents & POLLIN) { // Receive data
int len = recv(args->socket, buf, 2048, 0); int len = recv(args->socket, buf, 2048, 0);
if (len <= 0) {
if (len <= 0) { closing_message = "Lost peer (from recv)";
closing_message = "Lost peer";
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);
}
}
// Shutdown connection if we lost the device
if (event_poll->revents & POLLHUP || event_poll->revents & POLLERR) {
closing_message = "Lost device";
goto conn_end; goto conn_end;
} }
if (event_poll->revents & POLLIN) { // Parse message
struct input_event event; Message msg;
if (msg_deserialize(buf, len, &msg) != 0) {
printf("CONN(%d): Couldn't parse message.\n", args->id);
continue;
}
int len = read(dev.event, &event, sizeof(struct input_event)); // Handle message
if (msg.code == ControllerState) {
if (len <= 0) { int i = msg.controller_state.index;
closing_message = "Lost device"; if (i >= device_controllers.len) {
goto conn_end; printf("CONN(%d): Invalid controller index in controller state message\n", args->id);
}
if (len < sizeof(struct input_event)) {
printf("CONN(%d): error reading event\n", args->id);
continue; continue;
} }
if (event.type == EV_SYN) { Controller *ctr = *(Controller **)vec_get(&device_controllers, i);
int len = msg_serialize(buf, 2048, (Message *)&report); if (ctr == NULL) {
printf("CONN(%d): Received controller state message but the device hasn't yet been received\n", args->id);
if (len < 0) { continue;
printf("CONN(%d): Couldn't serialize report %d\n", args->id, len);
continue;
};
write(args->socket, buf, len);
} else if (event.type == EV_ABS) {
int index = dev.mapping.abs_indices[event.code];
if (index < 0) {
printf("CONN(%d): Invalid abs\n", args->id);
continue;
};
report.abs[index] = event.value;
} else if (event.type == EV_REL) {
int index = dev.mapping.rel_indices[event.code];
if (index < 0) {
printf("CONN(%d): Invalid rel\n", args->id);
continue;
};
report.rel[index] = event.value;
} else if (event.type == EV_KEY) {
int index = dev.mapping.key_indices[event.code];
if (index < 0) {
printf("CONN(%d): Invalid key\n", args->id);
continue;
};
report.key[index] = !!event.value;
} }
apply_controller_state(ctr, &msg.controller_state);
} else if (msg.code == Request) {
if (got_request) {
printf("CONN(%d): Illegal Request message after initial request\n", args->id);
msg_free(&msg);
continue;
}
got_request = true;
printf("CONN(%d): Got client request\n", args->id);
for (int i = 0; i < msg.request.request_count; i++) {
int index = device_controllers.len;
Controller *ctr = NULL;
vec_push(&device_controllers, &ctr);
struct DeviceThreadArgs *dev_args = malloc(sizeof(struct DeviceThreadArgs));
dev_args->controller = vec_get(&device_controllers, index);
dev_args->tag = strdup(msg.request.requests[i]);
dev_args->conn = args;
dev_args->index = index;
pthread_t thread;
pthread_create(&thread, NULL, device_thread, dev_args);
vec_push(&device_threads, &thread);
}
msg_free(&msg);
} else {
printf("CONN(%d): Illegal message\n", args->id);
} }
} }
conn_end: conn_end:
shutdown(args->socket, SHUT_RDWR); shutdown(args->socket, SHUT_RDWR);
printf("CONN(%u): connection closed (%s)\n", args->id, closing_message); printf("CONN(%u): connection closed (%s)\n", args->id, closing_message);
return_device(&dev);
free(args); free(args);
for (int i = 0; i < device_threads.len; i++) {
pthread_t thread = *(pthread_t *)vec_get(&device_threads, i);
pthread_kill(thread, SIGTERM);
pthread_join(thread, NULL);
}
return NULL; return NULL;
} }
void server_run(uint16_t port) { static int sockfd;
void clean_exit() {
printf("\rSERVER: exiting\n");
close(sockfd);
exit(0);
}
void server_run(uint16_t port, char *config_path) {
sigemptyset(&empty_sigset);
pthread_key_create(&device_args_key, NULL);
printf("SERVER: start\n"); printf("SERVER: start\n");
// Parse the config
{
FILE *configfd = fopen(config_path, "r");
if (configfd == NULL) {
perror("SERVER: Couldn't open config file");
exit(1);
}
char *cbuf = malloc(8192);
uint8_t *jbuf = (uint8_t *)cbuf + 4096;
int len = fread(cbuf, 1, 4096, configfd);
if (json_parse(cbuf, len, jbuf, 4096) != 0) {
printf("SERVER: Couldn't parse config, %s (at index %lu)\n", json_strerr(), json_errloc());
exit(1);
}
json_adapt(jbuf, &ConfigAdapter, &config);
free(cbuf);
fclose(configfd);
}
// Start the hid thread
{
pthread_t _thread;
pthread_create(&_thread, NULL, hid_thread, &config);
}
// Start the TCP server
int sock = socket(AF_INET, SOCK_STREAM, 0); int sock = socket(AF_INET, SOCK_STREAM, 0);
if (sock < 0) { if (sock < 0) {
panicf("Couldn't open socket\n"); panicf("Couldn't open socket\n");
} }
struct sockaddr_in addr = {}; sockfd = sock;
TRAP(SIGINT, clean_exit);
TRAP(SIGHUP, clean_exit);
TRAP(SIGQUIT, clean_exit);
TRAP(SIGTERM, clean_exit);
if (setsockopt(sock, SOL_SOCKET, SO_REUSEADDR, &(int){1}, sizeof(int)) < 0)
printf("SERVER: error when trying to enable SO_REUSEADDR\n");
if (setsockopt(sock, SOL_SOCKET, SO_REUSEPORT, &(int){1}, sizeof(int)) < 0)
printf("SERVER: error when trying to enable SO_REUSEPORT\n");
struct sockaddr_in addr = {0};
addr.sin_family = AF_INET; addr.sin_family = AF_INET;
addr.sin_addr.s_addr = htonl(INADDR_ANY); addr.sin_addr.s_addr = htonl(INADDR_ANY);
addr.sin_port = htons(port); addr.sin_port = htons(port);

28
server.h
View File

@ -1,8 +1,34 @@
// vi:ft=c // vi:ft=c
#ifndef SERVER_H_ #ifndef SERVER_H_
#define SERVER_H_ #define SERVER_H_
#include <stdbool.h>
#include <stdint.h> #include <stdint.h>
#include <time.h>
void server_run(uint16_t port); typedef struct {
// the 0 mac_address represents no filter
uint64_t mac_address;
// negative values means no filter
int32_t vendor;
// negative values means no filter
int32_t product;
bool js;
} ControllerFilter;
typedef struct {
ControllerFilter filter;
char *tag;
bool duplicate;
bool ps4_hidraw;
} ServerConfigController;
typedef struct {
ServerConfigController *controllers;
size_t controller_count;
struct timespec poll_interval;
uint32_t request_timeout;
} ServerConfig;
void server_run(uint16_t port, char *config_path);
#endif #endif

18
server_config.json Normal file
View File

@ -0,0 +1,18 @@
{
"controllers": [
{
"filter": { "mac_address": "00:b0:d0:63:c2:26" },
"tag": "Admin",
"properties": { "duplicate": true }
},
{
"filter": { "vendor": "054c", "product": "05c4", "js": true },
"tag": "User"
},
{
"filter": { "vendor": "046d", "product": "c332"},
"tag": "Mouse"
}
],
"poll_interval": 1
}

144
util.c
View File

@ -1,9 +1,11 @@
#include "util.h" #include "util.h"
#include <math.h>
#include <stdarg.h> #include <stdarg.h>
#include <stdint.h> #include <stdint.h>
#include <stdio.h> #include <stdio.h>
#include <stdlib.h> #include <stdlib.h>
#include <string.h>
#ifndef __has_builtin #ifndef __has_builtin
#define __has_builtin(_) 0 #define __has_builtin(_) 0
@ -34,3 +36,145 @@ uint8_t parse_hex_digit(char h) {
else else
return 0; return 0;
} }
// Defaults for json parsing
void default_to_null(void *ptr) { *(void **)ptr = NULL; }
void default_to_false(void *ptr) { *(bool *)ptr = false; }
void default_to_zero_u8(void *ptr) { *(uint8_t *)ptr = 0; }
void default_to_zero_u32(void *ptr) { *(uint32_t *)ptr = 0; }
void default_to_zero_u64(void *ptr) { *(uint64_t *)ptr = 0; }
void default_to_zero_size(void *ptr) { *(size_t *)ptr = 0; }
void default_to_zero_double(void *ptr) { *(double *)ptr = 0.0; }
void default_to_one_size(void *ptr) { *(size_t *)ptr = 1; }
void default_to_negative_one_i32(void *ptr) { *(int32_t *)ptr = -1; }
// Transformers for json parsing
void tsf_numsec_to_timespec(void *arg, void *ptr) {
double seconds = *(double *)arg;
struct timespec ts;
ts.tv_sec = floor(seconds);
ts.tv_nsec = (seconds - floor(seconds)) * 1000000000;
*(struct timespec *)ptr = ts;
}
void tsf_numsec_to_intms(void *arg, void *ptr) {
double seconds = *(double *)arg;
*(uint32_t *)ptr = seconds * 1000;
}
void tsf_strmac_to_u64(void *arg, void *ptr) {
char *s = *(char **)arg;
if (strnlen(s, 18) != 17) {
printf("JSON: wrong length for mac address, expected 'xx:xx:xx:xx:xx:xx'\n");
free(s);
return;
}
uint64_t mac = 0;
for (int i = 0; i < 17; i++) {
char c = s[i];
uint8_t 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 if (c == ':')
continue;
else {
printf("JSON: unexpected character '%c' in mac address at position %i (%s)\n", c, i, s);
free(s);
return;
}
mac <<= 4;
mac |= digit;
}
free(s);
*(uint64_t *)ptr = mac;
}
void tsf_hex_to_i32(void *arg, void *ptr) {
char *s = *(char **)arg;
char *f = s;
char c;
int32_t res = 0;
while ((c = *s++) != '\0') {
uint8_t 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 {
printf("JSON: unexpected character '%c' in hex string\n", c);
free(f);
return;
}
res <<= 4;
res |= digit;
}
free(f);
*(int32_t *)ptr = res;
}
void tsf_double_to_size(void *arg, void *ptr) {
double d = *(double *)arg;
*(size_t *)ptr = d;
}
static uint8_t hex_digit(char c) {
if (c >= '0' && c <= '9')
return c - '0';
else if (c >= 'a' && c <= 'f')
return c - 'a' + 10;
else if (c >= 'A' && c <= 'F')
return c - 'A' + 10;
else
return 16;
}
void tsf_hex_to_color(void *arg, void *ptr) {
char *s = *(char **)arg;
int len = strnlen(s, 8);
if (len != 7 || s[0] != '#') {
printf("JSON: bad hex color format expected '#RRGGBB' or '#rrggbb', got '%s'\n", s);
free(s);
return;
}
uint8_t digit[6] = {0};
uint8_t *color = ptr;
for (int i = 1; i < 7; i++) {
digit[i] = hex_digit(s[i]);
if (digit[i] == 16) {
printf("JSON: illegal character in hex color: '%c'\n", s[i]);
free(s);
return;
}
if (i % 2 == 0) {
uint8_t c = digit[i];
uint8_t p = digit[i - 1];
color[i / 2] = (p << 4) | c;
}
}
free(s);
}
void tsf_num_to_u8_clamp(void * arg, void *ptr) {
double n = *(double*)arg;
*(uint8_t*)ptr = n > 255.0 ? 255.0 : n < 0.0 ? 0.0 : n;
}
void tsf_num_to_int(void * arg, void *ptr) {
double n = *(double*)arg;
*(int*)ptr = n;
}

21
util.h
View File

@ -13,6 +13,27 @@ uint16_t parse_port(const char *str);
static inline bool bit_set(uint8_t *bits, int i) { return bits[i / 8] & (1 << (i % 8)); } static inline bool bit_set(uint8_t *bits, int i) { return bits[i / 8] & (1 << (i % 8)); }
// Align n to the next 8 boundary // Align n to the next 8 boundary
static inline size_t align_8(size_t n) { return (((n - 1) >> 3) + 1) << 3; } static inline size_t align_8(size_t n) { return (((n - 1) >> 3) + 1) << 3; }
// Align n to the next 8 boundary
static inline size_t align_2(size_t n) { return (((n - 1) >> 1) + 1) << 1; }
uint8_t parse_hex_digit(char h); uint8_t parse_hex_digit(char h);
void default_to_null(void *ptr);
void default_to_false(void *ptr);
void default_to_zero_u8(void *ptr);
void default_to_zero_u32(void *ptr);
void default_to_zero_u64(void *ptr);
void default_to_zero_size(void *ptr);
void default_to_zero_double(void *ptr);
void default_to_one_size(void *ptr);
void default_to_negative_one_i32(void *ptr);
void tsf_numsec_to_timespec(void *arg, void *ptr);
void tsf_numsec_to_intms(void *arg, void *ptr);
void tsf_strmac_to_u64(void *arg, void *ptr);
void tsf_hex_to_i32(void *arg, void *ptr);
void tsf_double_to_size(void * arg, void * ptr);
void tsf_hex_to_color(void *arg, void *ptr);
void tsf_num_to_u8_clamp(void * arg, void *ptr);
void tsf_num_to_int(void * arg, void *ptr);
#endif #endif

8
vec.c
View File

@ -40,7 +40,7 @@ static inline void vec_grow(Vec *v, size_t cap) {
void vec_push(Vec *v, void *data) { void vec_push(Vec *v, void *data) {
vec_grow(v, v->len + 1); vec_grow(v, v->len + 1);
memcpy(v->data + v->stride * v->len++, data, v->stride); memcpy((u_int8_t *)v->data + v->stride * v->len++, data, v->stride);
} }
void vec_pop(Vec *v, void *data) { void vec_pop(Vec *v, void *data) {
@ -68,7 +68,7 @@ void vec_insert(Vec *v, void *data, size_t index) {
} }
vec_grow(v, v->len + 1); vec_grow(v, v->len + 1);
void *slot = v->data + index * v->stride; uint8_t *slot = v->data + index * v->stride;
if (index < v->len) { if (index < v->len) {
memmove(slot + v->stride, slot, (v->len - index) * v->stride); memmove(slot + v->stride, slot, (v->len - index) * v->stride);
} }
@ -86,7 +86,7 @@ void vec_remove(Vec *v, size_t index, void *data) {
return; return;
} }
void *slot = v->data + index * v->stride; uint8_t *slot = v->data + index * v->stride;
if (data != NULL) { if (data != NULL) {
memcpy(data, slot, v->stride); memcpy(data, slot, v->stride);
} }
@ -103,7 +103,7 @@ void vec_extend(Vec *v, void *data, size_t len) {
return; return;
} }
vec_grow(v, v->len + len); vec_grow(v, v->len + len);
memcpy(v->data + v->stride * v->len, data, v->stride * len); memcpy((uint8_t*)v->data + v->stride * v->len, data, v->stride * len);
v->len += len; v->len += len;
} }

3
vec.h
View File

@ -2,11 +2,12 @@
#ifndef VEC_H_ #ifndef VEC_H_
#define VEC_H_ #define VEC_H_
#include <unistd.h> #include <unistd.h>
#include <stdint.h>
#define vec_of(type) vec_new(sizeof(type)) #define vec_of(type) vec_new(sizeof(type))
typedef struct { typedef struct {
void *data; uint8_t *data;
size_t cap; size_t cap;
size_t len; size_t len;
size_t stride; size_t stride;