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

Merge pull request #1 from viandoxdev/v2 

V2 - major refractor
This commit is contained in:
viandox 2022-10-15 17:30:25 +02:00 committed by GitHub
parent e371ef3ec4 c6bdbf7fd8
commit 3dacc54ef1
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
20 changed files with 1750 additions and 613 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
AlignConsecutiveDeclarations: true
AlignConsecutiveAssignments: true
AlignArrayOfStructures: Left
PointerAlignment: Right
ColumnLimit: 110
ColumnLimit: 130
IncludeBlocks: Regroup

11
Makefile
View File

@ -1,16 +1,23 @@
Q=@
CC=gcc
CFLAGS=-g -Wall -Wno-format-truncation -pthread -lm
GCCCFLAGS=-Wno-format-truncation
CFLAGS=-std=c11 -pedantic -g -Wall -pthread -D_GNU_SOURCE
LDFLAGS=-lm
BUILD_DIR=./objects
BIN=jsfw
RUNARGS=client localhost 7776
RUNARGS=server 7776 ./server_config.json
SOURCES=$(wildcard *.c)
OBJECTS:=$(patsubst %.c,$(BUILD_DIR)/%.o,$(SOURCES))
ifeq ($(CC),gcc)
CFLAGS:=$(CFLAGS) $(GCCCFLAGS)
endif
.PHONY: run
run: $(BIN)
@echo "RUN $(BIN) $(RUNARGS)"

175
README.md
View File

@ -1,38 +1,6 @@
# jsfw
Utility to forward uevent devices over network through a tcp connection.
# Usage
Start client:
```sh
jsfw client [server address] [server port]
```
Start server:
```sh
jsfw server [port]
```
When a device is connected to the server host, jsfw will notice it and assign it to one of the client which will in turn create a virtual device based on it.
The code can theoretically support any kind of device (mouse, keyboard, joystick...) but is artificially limited to PS4 controllers (see `hid.c::filter_event`), because the hidraw interface used to set additional device state (led color, flashing, rumble) only works with them. This could be easily edited tho (see `hid.c::apply_controller_state`, `net.h::MessageControllerState`, `net.c::{msg_serialize, msg_deserialize}` and `client.c::JControllerState`). To set the controller state from the client write the json state to the fifo (by default `/tmp/jsfw_fifo`).
The format for the controller state takes this form (comments not allowed):
```jsonc
{
"led_color": "#ff0000", // hex color string
"flash": [0.04, 0.11], // values are 0-1, first is time on second is time off
"rumble": [0, 0], // values are 0-1
}
```
Any value can be ommitted, extra values will be ignored.
Some aspect are easily configurable through `const.c`.
Utility to forward evdev devices over network through a tcp connection.
# Building
@ -49,3 +17,144 @@ make jsfw
```
output will be `./jsfw`.
# Usage
## Background
Jsfw works with a server and one or more clients, each client has a configuration and so does the server. The server configuration assigns tags to devices based on a set of filter, for example, you could give the "Controller" tag to devices of a certain vendor and product id. The client configuration specifies what the client wants: it is a set of tags as well as a few additional properties for the virtual devices. To illustrate that a little better, a simple setup would have a server with this configuration:
```json
{
"controllers": [
{
"filter": { "vendor": "1234", "product": "0000" },
"tag": "Controller"
}
],
}
```
and a client with this one:
```json
{
"controllers": [
{
"tag": "Controller"
}
]
}
```
This would result in the server picking up any device with the vendor id `1234` and the product id `0000`, taging it as a `Controller` and forwarding it to the client when it connects.
## Configuration
Jsfw makes use of two json configurations to dictate the behaviour of both the server and client.
### Server
The server configuration specifies some of the server settings, as well as which devices to assign to which tag.
```js
// Any property can be ommitted, unless specified otherwise
// The values listed here are examples
{
"controllers": [
{
// (required) The tag to assign the to devices matching the filter
"tag": "Joystick",
// (default: accept any device) Requirements for a device to be assigned the tag
"filter": {
// (default: none) Optionally match the uniq of a device, expects a 17 long string of this form
"uniq": "aa:bb:cc:dd:ee:ff",
// (default: none) Optionally match the vendor code of the device, expects a 4 long hex string
"vendor": "054c",
// (default: none) Optionally match the product code of the device, expects a 4 long hex string
"product": "abcd",
// (default: false) Wether to check for a js* entry in the device tree, useful to match only the
// controller when a device has multiple events (i.e in the case of a ps4 controller one device has
// a js and is the controller, and another, with the same uniq/vendor/product, is the mouse and keyboard).
"js": true,
// (default: none) Optionally match the name of the device
"name": "Asus keyboard"
},
// Additional properties for the jsfw behaviour, some properties may act as a filter.
"properties": {
// (default: false) Wether this device can be shared by multiple client
"duplicate": true,
// (default: false) Wether the devices are dualshock 4 controllers that can be controlled
// through the hidraw interface, this allows changing the led colors from the client by writing
// json to the fifo (see client configuration). If this is enabled, any device whose hidraw interface
// can't be found will be filtered out
"ps4_hidraw": true
}
}
],
// (default: 1s) Number of seconds between each poll for physical devices
"poll_interval": 2.5,
// (default: 2s) Number of seconds to wait for a client's request before closing the connection
"request_timeout": 10
}
```
The client configuration specifies what devices the client wants as well as under what name/properties should they appear
```js
// Any property can be ommitted, unless specified otherwise
// The values listed here are examples
{
"controllers": [
{
// (required) Tag of the device to request
"tag": "Joystick",
// (default: 6969) Vendor code for the virtual device, expects a 4 long hex string
"vendor": "dead",
// (default: 0420) Product code for the virtual device, expects a 4 long hex string
"product": "beef",
// (default: "JSFW Virtual Device") Name for the virtual device
"name": "Emanuel"
}
],
// (default: "/tmp/jsfw_fifo") Path to the fifo for hidraw
"fifo_path": "/tmp/gaming",
// (default: 5s) Number of seconds between retries when connecting to the server
"retry_delay": 2.5
}
```
Additionaly messages can be sent to the client's fifo to change the led colors (and more) of ps4\_hidraw devices, these take the form:
```js
// Any property can be ommitted, unless specified otherwise
// The values listed here are examples
{
// (default: 0) Index of the device to send the state to, this is the index in the client configuration controllers list
"index": 1,
// (default: [0, 0]) Setting for the rumble, values are in range 0-255 first element is small rumble, second is big
"rumble": [255, 0],
// (default: [0, 0]) Setting for led flash, values are in range 0-255, first element is led on, second is led off
"false": [0, 255],
// (default: "#FFFFFF") Hex color for the led
"led_color": "#FF0000"
}
```
## Execution
To start the server:
```sh
./jsfw server [port] [path to config]
```
To start the client:
```sh
./jsfw client [address] [port] [path to config]
```
# Contributing
lol, lmao even. (nah just issues/PR I guess)

348
client.c
View File

@ -4,13 +4,13 @@
#include "json.h"
#include "net.h"
#include "util.h"
#include "vec.h"
#include <arpa/inet.h>
#include <fcntl.h>
#include <linux/input-event-codes.h>
#include <linux/input.h>
#include <linux/uinput.h>
#include <math.h>
#include <netinet/in.h>
#include <poll.h>
#include <stdbool.h>
@ -25,17 +25,10 @@
#include <time.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 struct sockaddr_in server_addr = {};
static char server_addrp[64] = {};
static struct sockaddr_in server_addr = {0};
static char server_addrp[64] = {0};
static uint16_t server_port = -1;
static struct pollfd poll_fds[2];
@ -45,57 +38,114 @@ static int fifo = -1;
static int sock = -1;
// static to avoid having this on the stack because a message is about 2kb in memory
static Message message;
static VirtualDevice device = {};
// Test if the device exists
static inline bool device_exists() { return device.fd > 0; }
static Vec devices_fd;
static Vec devices_info;
// Struct representing the received json
typedef struct {
char *led_color;
double rumble_small;
double rumble_big;
double flash_on;
double flash_off;
} JControllerState;
static ClientConfig config;
static MessageRequest device_request;
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)},
static void default_fifo_path(void *ptr) { *(char **)ptr = (char *)FIFO_PATH; }
static void default_retry_delay(void *ptr) { *(struct timespec *)ptr = CONNECTION_RETRY_DELAY; }
static void default_vendor(void *ptr) { *(int32_t *)ptr = VIRTUAL_DEVICE_VENDOR; }
static void default_product(void *ptr) { *(int32_t *)ptr = VIRTUAL_DEVICE_PRODUCT; }
static void default_name(void *ptr) { *(char **)ptr = (char *)VIRTUAL_DEVICE_NAME; }
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
void device_destroy() {
if (!device_exists()) {
static const JSONPropertyAdapter ControllerAdapterProps[] = {
{".tag", &StringAdapter, offsetof(ClientController, tag), default_to_null, NULL },
{".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(void) {
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;
}
ioctl(device.fd, UI_DEV_DESTROY);
close(device.fd);
device.fd = -1;
printf("CLIENT: Destroyed device\n");
int fd = *(int *)vec_get(&devices_fd, index);
MessageDeviceInfo *info = vec_get(&devices_info, index);
if (info->code == DeviceInfo) {
ioctl(fd, UI_DEV_DESTROY);
info->code = NoMessage;
}
}
// (Re)Initialize the device
void device_init(MessageDeviceInfo *dev) {
device_destroy();
int fd = open("/dev/uinput", O_WRONLY | O_NONBLOCK);
if (fd < 0) {
perror("CLIENT: Error while opening /dev/uinput, ");
exit(1);
if (dev->index >= devices_info.len) {
printf("CLIENT: Got wrong device index\n");
return;
}
// Setup device_info
device_destroy(dev->index);
int fd = *(int *)vec_get(&devices_fd, dev->index);
// Abs
if (dev->abs_count > 0) {
ioctl(fd, UI_SET_EVBIT, EV_ABS);
for (int i = 0; i < dev->abs_count; i++) {
struct uinput_abs_setup setup = {};
struct uinput_abs_setup setup = {0};
setup.code = dev->abs_id[i];
setup.absinfo.minimum = dev->abs_min[i];
setup.absinfo.maximum = dev->abs_max[i];
@ -123,78 +173,106 @@ 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.vendor = VIRTUAL_DEVICE_VENDOR;
setup.id.product = VIRTUAL_DEVICE_PRODUCT;
setup.id.vendor = ctr->device_vendor;
setup.id.product = ctr->device_product;
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_CREATE);
device.fd = fd;
memcpy(&device.info, dev, sizeof(MessageDeviceInfo));
printf("CLIENT: Created device (abs: %d, rel: %d, key: %d)\n", dev->abs_count, dev->rel_count,
dev->key_count);
MessageDeviceInfo *dst = vec_get(&devices_info, dev->index);
memcpy(dst, dev, sizeof(MessageDeviceInfo));
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
int device_emit(uint16_t type, uint16_t id, uint32_t value) {
struct input_event event = {};
bool device_emit(int index, uint16_t type, uint16_t id, uint32_t value) {
if (index >= devices_fd.len) {
return true;
}
int fd = *(int *)vec_get(&devices_fd, index);
struct input_event event = {0};
event.type = type;
event.code = id;
event.value = value;
return write(device.fd, &event, sizeof(event)) != sizeof(event);
return write(fd, &event, sizeof(event)) != sizeof(event);
}
// Update device with report
void device_handle_report(MessageDeviceReport *report) {
if (!device_exists()) {
printf("CLIENT: Got report before device info\n");
if (!device_exists(report->index)) {
printf("CLIENT: [%d] Got report before device info\n", report->index);
return;
}
if (report->abs_count != device.info.abs_count || report->rel_count != device.info.rel_count ||
report->key_count != device.info.key_count) {
MessageDeviceInfo *info = vec_get(&devices_info, report->index);
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");
return;
}
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");
}
}
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");
}
}
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");
}
}
// 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
device_emit(EV_SYN, 0, 0);
device_emit(report->index, EV_SYN, 0, 0);
}
void setup_fifo();
void setup_devices(void) {
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);
// (Re)Open the fifo
void open_fifo() {
void open_fifo(void) {
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) {
fifo_attempt++;
unlink(FIFO_PATH);
unlink(config.fifo_path);
setup_fifo();
} else if (fifo < 0) {
panicf("CLIENT: Couldn't open fifo, aborting\n");
@ -203,9 +281,9 @@ void open_fifo() {
}
// Ensure the fifo exists and opens it (also setup poll_fd)
void setup_fifo() {
void setup_fifo(void) {
mode_t prev = umask(0);
mkfifo(FIFO_PATH, 0666);
mkfifo(config.fifo_path, 0666);
umask(prev);
open_fifo();
@ -215,12 +293,12 @@ void setup_fifo() {
}
// (Re)Connect to the server
void connect_server() {
while (1) {
void connect_server(void) {
while (true) {
if (sock > 0) {
// Close previous connection
device_destroy();
shutdown(sock, SHUT_RDWR);
destroy_devices();
close(sock);
}
@ -230,11 +308,9 @@ void connect_server() {
}
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,
CONNECTION_RETRY_DELAY);
struct timespec ts = {};
ts.tv_sec = CONNECTION_RETRY_DELAY;
nanosleep(&ts, NULL);
printf("CLIENT: Couldn't connect to %s:%d, retrying in %lu.%09lus\n", server_addrp, server_port,
config.retry_delay.tv_sec, config.retry_delay.tv_nsec);
nanosleep(&config.retry_delay, NULL);
continue;
}
// 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);
socket_poll->fd = sock;
printf("CLIENT: Connected !\n");
uint8_t buf[2048] __attribute__((aligned(4))) = {0};
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;
}
}
@ -264,21 +350,44 @@ void setup_server(char *address, uint16_t port) {
connect_server();
}
void early_checks() {
int fd = open("/dev/uinput", O_WRONLY | O_NONBLOCK);
if (fd < 0) {
perror("CLIENT: Can't open /dev/uinput, aborting now: ");
void build_device_request(void) {
char **tags = malloc(config.controller_count * sizeof(char *));
for (int i = 0; i < config.controller_count; i++) {
tags[i] = config.controllers[i].tag;
}
device_request.code = Request;
device_request.request_count = config.controller_count;
device_request.requests = tags;
}
void client_run(char *address, uint16_t port, char *config_path) {
// Parse the config
{
FILE *configfd = fopen(config_path, "r");
if (configfd == NULL) {
perror("CLIENT: Couldn't open config file");
exit(1);
}
close(fd);
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);
}
void client_run(char *address, uint16_t port) {
// Device doesn't exist yet
device.fd = -1;
json_adapt(jbuf, &ConfigAdapter, &config);
free(cbuf);
fclose(configfd);
}
early_checks();
setup_fifo();
build_device_request();
setup_devices();
setup_server(address, port);
uint8_t buf[2048] __attribute__((aligned(4)));
@ -287,7 +396,7 @@ void client_run(char *address, uint16_t port) {
while (1) {
int rc = poll(poll_fds, 2, -1);
if (rc < 0) {
perror("CLIENT: Error on poll, ");
perror("CLIENT: Error on poll");
exit(1);
}
@ -299,55 +408,19 @@ void client_run(char *address, uint16_t port) {
// We've got data from the fifo
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_errloc());
printf("CLIENT: Error when parsing fifo message as json (%s at index %lu)\n", json_strerr(), json_errloc());
} 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);
}
json_adapt(json_buf, &ControllerStateAdapter, &msg);
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);
"(%d, %d) -> [%d]\n",
msg.led[0], msg.led[1], msg.led[2], msg.flash_on, msg.flash_off, msg.small_rumble,
msg.big_rumble, msg.index);
};
};
}
@ -356,19 +429,20 @@ void client_run(char *address, uint16_t port) {
// A broken or closed socket produces a POLLIN event, so we check for error on the recv
if (socket_poll->revents & POLLIN) {
int len = recv(sock, buf, 2048, 0);
int len = recv(sock, buf, 2048, MSG_PEEK);
if (len <= 0) {
printf("CLIENT: Lost connection to server, reconnecting\n");
shutdown(sock, SHUT_RDWR);
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)
continue;
}
int msg_len = msg_deserialize(buf, len, &message);
// We've got data from the server
if (msg_deserialize(buf, len, &message) != 0) {
printf("CLIENT: Couldn't parse message (code: %d, len: %d)\n", buf[0], len);
if (msg_len < 0) {
recv(sock, buf, 2048, 0);
printf("CLIENT: Couldn't parse message (code: %d, len: %d)\n", buf[4], len);
int l = len > 100 ? 100 : len;
for (int i = 0; i < l; i++) {
@ -383,14 +457,20 @@ void client_run(char *address, uint16_t port) {
continue;
}
recv(sock, buf, msg_len, 0);
if (message.code == DeviceInfo) {
if (device_exists()) {
printf("CLIENT: Got more than one device info\n");
if (device_exists(message.device_info.index)) {
printf("CLIENT: Got more than one device info for same device\n");
}
device_init((MessageDeviceInfo *)&message);
printf("CLIENT: Got device %d\n", message.device_info.index);
} else if (message.code == DeviceReport) {
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 {
printf("CLIENT: Illegal message\n");
}

18
client.h
View File

@ -2,7 +2,23 @@
#ifndef CLIENT_H_
#define CLIENT_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

7
const.c
View File

@ -3,14 +3,19 @@
#include <stdint.h>
#include <time.h>
// These are default values for the most part
// Any value updated here should also be updated in README.md
// How long between each device poll
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
const char *DEVICE_DEFAULT_NAME = "Unnamed Device";
// Path to the fifo
const char *FIFO_PATH = "/tmp/jsfw_fifo";
// 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
const uint16_t VIRTUAL_DEVICE_VENDOR = 0x6969;
// Displayed product for the virtual device

3
const.h
View File

@ -5,9 +5,10 @@
#include <time.h>
extern const struct timespec POLL_DEVICE_INTERVAL;
extern const int REQUEST_TIMEOUT;
extern const char *DEVICE_DEFAULT_NAME;
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_PRODUCT;
extern const uint16_t VIRTUAL_DEVICE_VERSION;

356
hid.c
View File

@ -1,6 +1,7 @@
#include "hid.h"
#include "const.h"
#include "server.h"
#include "util.h"
#include "vec.h"
@ -17,18 +18,20 @@
#include <time.h>
#include <unistd.h>
// List of uniq of the currently known devices
static Vec devices;
// List of the new devices of a poll, static to keep the allocation alive
static Vec new_devices;
// Queue of devices to be taken by connections
static Vec devices_queue;
// 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;
// List of ids of the currently known devices
static Vec known_devices;
// Queue of available devices, devices that can only be given to one client
static Vec available_devices;
// List of cloneable devices, devices that can be handed out to multiple clients
static Vec cloneable_devices;
// Mutex for devices
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
uniq_t parse_uniq(char uniq[17]) {
@ -64,8 +67,8 @@ void setup_device(PhysicalDevice *dev) {
for (int i = 0; i < KEY_CNT; i++)
dev->mapping.key_indices[i] = -1;
uint8_t type_bits[EV_MAX] = {};
uint8_t feat_bits[(KEY_MAX + 7) / 8] = {};
uint8_t type_bits[EV_MAX] = {0};
uint8_t feat_bits[(KEY_MAX + 7) / 8] = {0};
ioctl(dev->event, EVIOCGBIT(0, EV_MAX), type_bits);
// 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.
// This is pretty arbritrary
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)
{
bool filter_event(int fd, char *event, ControllerFilter *filter, uniq_t uniq) {
if (filter->js) {
char device_path[64];
snprintf(device_path, 64, "/sys/class/input/%s/device", event);
@ -144,85 +142,131 @@ bool filter_event(int fd, char *event) {
}
}
// Check product and vendor id 054c:05c4 => Dualshock 4 (09cc is for the second generation)
uint16_t info[4];
ioctl(fd, EVIOCGID, info);
return info[1] == 0x054c && (info[2] == 0x05c4 || info[2] == 0x09cc);
if (filter->name != NULL) {
char name[256] = {0};
ioctl(fd, EVIOCGNAME(256), name);
if (strcmp(name, filter->name) != 0) {
return false;
}
}
if (filter->uniq > 0 && uniq != filter->uniq) {
return false;
}
struct input_id ids;
ioctl(fd, EVIOCGID, &ids);
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
void poll_devices_init() {
devices = vec_of(uniq_t);
new_devices = vec_of(PhysicalDevice);
devices_queue = vec_of(PhysicalDevice);
void poll_devices_init(void) {
known_devices = vec_of(Controller);
cloneable_devices = vec_of(Controller *);
available_devices = vec_of(Controller *);
}
// Block to get a device, this is thread safe
PhysicalDevice get_device() {
// stop: additional condition to check before doing anything,
// if the condition is ever found to be true the function will return immediately with a NULL pointer.
Controller *get_device(char *tag, bool *stop) {
// Check if we can get one right away
pthread_mutex_lock(&devices_queue_mutex);
if (devices_queue.len > 0) {
PhysicalDevice r;
vec_pop(&devices_queue, &r);
pthread_mutex_unlock(&devices_queue_mutex);
return r;
}
// 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:
// - If the connection to a client is lost, the device is forgotten, picked up by the next device poll, and
// put back in the queue
// - If the device dies (i.e unplugged), the connection to the client is closed and the device forgotten.
//
// 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);
}
// try to close the file descriptor, they may be already closed if the device was unpugged.
close(dev->event);
close(dev->hidraw);
// Safely remove device from the known device list
pthread_mutex_lock(&devices_mutex);
for (int i = 0; i < devices.len; i++) {
uniq_t *uniq = vec_get(&devices, i);
if (*uniq == dev->uniq) {
vec_remove(&devices, i, NULL);
while (1) {
if (*stop) {
pthread_mutex_unlock(&devices_mutex);
return NULL;
}
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);
}
}
// Return a device that isn't used anymore, this really only makes sense for non cloneable devices.
void return_device(Controller *c) {
// If device is cloneable there is nothing to return
if (c->ctr.duplicate) {
return;
}
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.id == c->dev.id) {
vec_remove(&cloneable_devices, i, NULL);
break;
}
}
pthread_mutex_unlock(&devices_mutex);
}
// Find all available devices and pick up on new ones
void poll_devices() {
vec_clear(&new_devices);
// Free the name if it was allocated
if (c->dev.name != NULL && c->dev.name != DEVICE_DEFAULT_NAME) {
printf("HID: Forgetting device '%s' (%016lx)\n", c->dev.name, c->dev.id);
free(c->dev.name);
} else {
printf("HID: Forgetting device %016lx\n", c->dev.id);
}
// try to close the file descriptor, they may be already closed if the device was unpugged.
close(c->dev.event);
close(c->dev.hidraw);
// Safely remove device from the known device list
pthread_mutex_lock(&known_devices_mutex);
for (int i = 0; i < known_devices.len; i++) {
Controller *d = vec_get(&known_devices, i);
if (d->dev.id == c->dev.id) {
vec_remove(&known_devices, i, NULL);
break;
}
}
pthread_mutex_unlock(&known_devices_mutex);
}
// Find all available devices and pick up on new ones
void poll_devices(void) {
// loop over all entries of /sys/class/input
DIR *input_dir = opendir("/sys/class/input");
struct dirent *input;
while ((input = readdir(input_dir)) != NULL) {
// 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) {
@ -230,8 +274,11 @@ void poll_devices() {
}
PhysicalDevice dev;
dev.hidraw = -1;
dev.uniq = 0;
// Open /dev/input/eventXX
{
char event_path[64];
snprintf(event_path, 64, "/dev/input/%s", input->d_name);
@ -240,55 +287,77 @@ void poll_devices() {
if (dev.event < 0) { // Ignore device if we couldn't open
continue;
}
}
// Try to get the name, default to DEFAULT_NAME if impossible
char name_buf[256] = {};
const char *name;
char *name;
{
static char name_buf[256] = {0};
if (ioctl(dev.event, EVIOCGNAME(256), name_buf) >= 0) {
name = name_buf;
} else {
name = DEVICE_DEFAULT_NAME;
name = (char *)DEVICE_DEFAULT_NAME;
}
// Filter events we don't care about
if (!filter_event(dev.event, input->d_name)) {
goto skip;
}
// 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);
uniq = parse_uniq(uniq_str);
// If we couldn't parse the uniq (this assumes uniq can't be zero, which is probably alright)
if (uniq == 0) {
goto skip;
}
dev.uniq = parse_uniq(uniq_str);
}
// Check if we already know of this device
bool found = false;
// Used for linear searches
bool found;
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) {
// 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, dev.uniq)) {
found = true;
break;
}
}
pthread_mutex_unlock(&devices_mutex);
if (!found) {
goto skip;
}
}
// Get the id
{
struct input_id id;
ioctl(dev.event, EVIOCGID, &id);
dev.id = *(uint64_t *)&id;
}
// Check if we already know of this device
{
found = false;
pthread_mutex_lock(&known_devices_mutex);
for (int i = 0; i < known_devices.len; i++) {
Controller *c = vec_get(&known_devices, i);
if (c->dev.id == dev.id) {
found = true;
break;
}
}
pthread_mutex_unlock(&known_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
// Look for hidraw if the device should have one (Dualshock 4 only, with ps4_hidraw property set)
if (ctr->ps4_hidraw) {
// Attempt to find the path
char hidraw_path[64];
{
char hidraw_dir_path[256];
@ -303,20 +372,22 @@ void poll_devices() {
}
if (hidraw == NULL) {
printf("Couldn't get hidraw of %s", input->d_name);
continue;
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) {
dev.name = malloc(256);
@ -327,40 +398,55 @@ void poll_devices() {
}
}
// 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(&known_devices_mutex);
// Index of the device in known_devices
int index = known_devices.len;
vec_push(&known_devices, &c);
pthread_mutex_unlock(&known_devices_mutex);
// Pointer to the device in known_devices
Controller *p = vec_get(&known_devices, index);
printf("HID: New device, %s [%s] (%s: %016lx)\n", name, ctr->tag, input->d_name, dev.id);
if (ctr->duplicate) {
pthread_mutex_lock(&devices_mutex);
vec_push(&devices, &uniq);
vec_push(&cloneable_devices, &p);
// Signal that there are new cloneable devices
pthread_cond_broadcast(&devices_cond);
pthread_mutex_unlock(&devices_mutex);
vec_push(&new_devices, &dev);
printf("HID: New device, %s (%s: %012lx)\n", name, input->d_name, dev.uniq);
// Continue here to avoid running cleanup code of skip
} 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;
// close open file descriptor and continue
skip:
close(dev.event);
continue;
};
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
void apply_controller_state(PhysicalDevice *dev, MessageControllerState *state) {
printf("HID: (%012lx) Controller state: #%02x%02x%02x flash: (%d, %d) rumble: (%d, %d)\n", dev->uniq,
state->led[0], state->led[1], state->led[2], state->flash_on, state->flash_off,
state->small_rumble, state->big_rumble);
// "Execute" a MessageControllerState: set the led color, rumble and flash using the hidraw interface (Dualshock 4 only)
void apply_controller_state(Controller *c, MessageControllerState *state) {
if (c->ctr.ps4_hidraw && c->dev.hidraw < 0) {
printf("HID: Trying to apply controller state on incompatible device (%016lx)\n", c->dev.id);
return;
}
printf("HID: (%016lx) Controller state: #%02x%02x%02x flash: (%d, %d) rumble: (%d, %d)\n", c->dev.id, 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};
@ -372,24 +458,24 @@ void apply_controller_state(PhysicalDevice *dev, MessageControllerState *state)
buf[9] = state->flash_on;
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) {
// May not be necessary
fsync(dev->hidraw);
fsync(c->dev.hidraw);
// Send a second time, to reenable the led
write(dev->hidraw, buf, 32);
write(c->dev.hidraw, buf, 32);
};
}
// Body of the hid thread
void *hid_thread() {
void *hid_thread(void *arg) {
printf("HID: start\n");
config = arg;
poll_devices_init();
while (1) {
poll_devices();
nanosleep(&POLL_DEVICE_INTERVAL, NULL);
nanosleep(&config->poll_interval, NULL);
}
return NULL;

17
hid.h
View File

@ -2,8 +2,10 @@
#ifndef HID_H_
#define HID_H_
#include "net.h"
#include "server.h"
#include <linux/input-event-codes.h>
#include <stdbool.h>
#include <stdint.h>
// Unique identifier for devices (provided by linux), May be the mac address
@ -24,14 +26,21 @@ typedef struct {
int event;
int hidraw;
uniq_t uniq;
uint64_t id;
char *name;
DeviceMap mapping;
MessageDeviceInfo device_info;
} PhysicalDevice;
void *hid_thread();
void return_device(PhysicalDevice *dev);
PhysicalDevice get_device();
void apply_controller_state(PhysicalDevice *dev, MessageControllerState *state);
typedef struct {
PhysicalDevice dev;
ServerConfigController ctr;
} Controller;
void *hid_thread(void *arg);
void return_device(Controller *c);
void forget_device(Controller *c);
Controller *get_device(char *tag, bool *stop);
void apply_controller_state(Controller *c, MessageControllerState *state);
#endif

244
json.c
View File

@ -16,11 +16,21 @@ static JSONError jerrno = NoError;
static size_t jerr_index = 0;
// Get a string explaining the last json parsing error
const char *json_strerr() { return JSONErrorMessage[jerrno]; }
const char *json_strerr(void) { return JSONErrorMessage[jerrno]; }
// Get the code of the last json parsing error
JSONError json_errno() { return jerrno; }
JSONError json_errno(void) { return jerrno; }
// Get the location of the last json parsing error
size_t json_errloc() { return jerr_index; }
size_t json_errloc(void) { 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;
// i.e
@ -45,8 +55,7 @@ static int json_parse_value(const char **buf, const char *buf_end, uint8_t **res
const uint8_t *dst_end); // Declaration for recursion
// *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) {
static inline int json_parse_string(const char **buf, const char *buf_end, uint8_t **restrict dst, const uint8_t *dst_end) {
// Ensure enough space for the header
if (*dst + sizeof(JSONHeader) >= dst_end) {
return set_jerrno(DstOverflow);
@ -111,27 +120,27 @@ static inline int json_parse_string(const char **buf, const char *buf_end, uint8
return set_jerrno(DstOverflow);
}
*(*dst)++ = 0b11000000 | (un_codepoint >> 6 & 0b011111);
*(*dst)++ = 0b10000000 | (un_codepoint >> 0 & 0b111111);
*(*dst)++ = 0xC0 | (un_codepoint >> 6 & 0x1F);
*(*dst)++ = 0x80 | (un_codepoint >> 0 & 0x3F);
header->len += 2;
} else if (un_codepoint <= 0xffff) { // 3 byte codepoint
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);
*(*dst)++ = 0xE0 | (un_codepoint >> 12 & 0x0F);
*(*dst)++ = 0x80 | (un_codepoint >> 6 & 0x3F);
*(*dst)++ = 0x80 | (un_codepoint >> 0 & 0x3F);
header->len += 3;
} else if (un_codepoint <= 0x10ffff) { // 4 byte codepoint
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);
*(*dst)++ = 0xF0 | (un_codepoint >> 18 & 0x07);
*(*dst)++ = 0x80 | (un_codepoint >> 12 & 0x3F);
*(*dst)++ = 0x80 | (un_codepoint >> 6 & 0x3F);
*(*dst)++ = 0x80 | (un_codepoint >> 0 & 0x3F);
header->len += 4;
} else { // Illegal codepoint
return set_jerrno(StringBadUnicode);
@ -198,8 +207,7 @@ static inline int json_parse_string(const char **buf, const char *buf_end, uint8
(*buf)++;
return 0;
} else if ((c < ' ' && c != '\t') ||
c == 0x7f) { // Illegal characters, technically tab isn't allowed either
} else if ((c < ' ' && c != '\t') || c == 0x7f) { // Illegal characters, technically tab isn't allowed either
// but it felt weird so I added it
jerrno = StringBadChar;
return -1;
@ -219,8 +227,7 @@ static inline int json_parse_string(const char **buf, const char *buf_end, uint8
}
// *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) {
static int json_parse_number(const char **buf, const char *buf_end, uint8_t **restrict dst, const uint8_t *dst_end) {
// Ensure enough space for header and value
if (*dst + sizeof(JSONHeader) + sizeof(double) >= dst_end) {
return set_jerrno(DstOverflow);
@ -339,8 +346,7 @@ static int json_parse_number(const char **buf, const char *buf_end, uint8_t **re
}
// *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) {
static int json_parse_boolean(const char **buf, const char *buf_end, uint8_t **restrict dst, const uint8_t *dst_end) {
// Ensure enough space for header and value
if (*dst + sizeof(JSONHeader) + 8 >= dst_end) { // 8: sizeof(uint64_t)
return set_jerrno(DstOverflow);
@ -379,8 +385,7 @@ static int json_parse_boolean(const char **buf, const char *buf_end, uint8_t **r
}
// *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) {
static int json_parse_null(const char **buf, const char *buf_end, uint8_t **restrict dst, const uint8_t *dst_end) {
// Ensure enough size for the header (no value)
if (*dst + sizeof(JSONHeader) >= dst_end) {
return set_jerrno(DstOverflow);
@ -405,8 +410,7 @@ static int json_parse_null(const char **buf, const char *buf_end, uint8_t **rest
}
// *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) {
static int json_parse_array(const char **buf, const char *buf_end, uint8_t **restrict dst, const uint8_t *dst_end) {
// Ensure enough space for the header
if (*dst + sizeof(JSONHeader) >= dst_end) {
return set_jerrno(DstOverflow);
@ -431,6 +435,7 @@ static int json_parse_array(const char **buf, const char *buf_end, uint8_t **res
}
if (**buf == ']') { // Array is empty
(*buf)++;
header->len = 0;
return 0;
}
@ -466,8 +471,7 @@ static int json_parse_array(const char **buf, const char *buf_end, uint8_t **res
}
// *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) {
static int json_parse_object(const char **buf, const char *buf_end, uint8_t **restrict dst, const uint8_t *dst_end) {
// Esnure enough space for the header
if (*dst + sizeof(JSONHeader) >= dst_end) {
return set_jerrno(DstOverflow);
@ -490,6 +494,7 @@ static int json_parse_object(const char **buf, const char *buf_end, uint8_t **re
return set_jerrno(SrcOverflow);
}
if (**buf == '}') {
(*buf)++;
// The object is empty
header->len = 0;
return 0;
@ -545,8 +550,7 @@ static int json_parse_object(const char **buf, const char *buf_end, uint8_t **re
}
// *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) {
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)++) {
// Ignore initial whitespaces
if (is_whitespace(**buf))
@ -661,49 +665,185 @@ void json_print_value_priv(uint8_t **buf) {
// /!\ doesn't handle strings well
void json_print_value(uint8_t *buf) { json_print_value_priv(&buf); }
// Loop over adapters and set accordingly
static void json_adapt_set(uint8_t *buf, JSONAdapter *adapters, size_t adapter_count, void *ptr, char *path) {
void json_print_buffer(uint8_t *buf) {
uint8_t *end = buf + align_8(((JSONHeader *)buf)->len) + sizeof(JSONHeader);
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 (strcmp(path, adapters[i].path) == 0 && header->type == adapters[i].type) {
void *p = ptr + adapters[i].offset;
if (header->type == Object || header->type == Array)
continue;
printf("[\033[34m");
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));
printf("%lf", *(double *)buf);
break;
case Boolean:
*(bool *)p = *(uint64_t *)(buf + sizeof(JSONHeader)) == 1;
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]");
buf += align_8(header->len);
}
printf("\n");
}
static inline bool ends_with(const char *str, const char *pat) {
size_t strl = strlen(str);
size_t patl = strlen(pat);
return strl >= patl && strcmp(str + strl - patl, pat) == 0;
}
static void json_adapt_set_defaults(const JSONAdapter *adapter, void *ptr) {
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
static void json_adapt_priv(uint8_t **buf, JSONAdapter *adapters, size_t adapter_count, void *ptr,
char *full_path, char *path) {
// 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) {
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) {
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:
@ -714,7 +854,7 @@ static void json_adapt_priv(uint8_t **buf, JSONAdapter *adapters, size_t adapter
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);
json_adapt_priv(buf, adapter, ptr, path_buffer, full_path, path + len);
}
} break;
case Object: {
@ -727,14 +867,14 @@ static void json_adapt_priv(uint8_t **buf, JSONAdapter *adapters, size_t adapter
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);
json_adapt_priv(buf, adapter, ptr, path_buffer, full_path, path + len);
}
} break;
}
}
// Run adapters on a json value
void json_adapt(uint8_t *buf, JSONAdapter *adapters, size_t adapter_count, void *ptr) {
// Run adapter on a json value
void json_adapt(uint8_t *buf, const JSONAdapter *adapter, void *ptr) {
char path[512] = ".";
json_adapt_priv(&buf, adapters, adapter_count, ptr, path, path);
json_adapt_priv(&buf, adapter, ptr, path, path, path);
}

63
json.h
View File

@ -2,6 +2,7 @@
#ifndef JSON_H_
#define JSON_H_
#include <limits.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
@ -37,6 +38,34 @@ typedef enum {
JERRORNO_MAX = 10
} JSONError;
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(void);
size_t json_errloc(void);
JSONError json_errno(void);
extern const JSONAdapter NumberAdapter;
extern const JSONAdapter StringAdapter;
extern const JSONAdapter BooleanAdapter;
#ifdef JSON_C_
static const char *JSONErrorMessage[JERRORNO_MAX + 1] = {
"No error",
@ -51,20 +80,26 @@ static const char *JSONErrorMessage[JERRORNO_MAX + 1] = {
"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
// 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

31
main.c
View File

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

268
net.c
View File

@ -1,18 +1,36 @@
#include "net.h"
#include "util.h"
#include <stdbool.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
// and writes result to dst
int msg_deserialize(const uint8_t *buf, size_t len, Message *restrict dst) {
{
if (len <= MAGIC_SIZE) {
return -1;
}
if (*(MAGIC_TYPE *)buf != MAGIC_BEG) {
printf("NET: No magic in message\n");
return -1;
}
buf += MAGIC_SIZE;
len -= MAGIC_SIZE;
}
// Decrement len so that it becomes the len of the data without the code.
if (len-- < 1)
return -1;
// This ensures that only a byte is read instead of a full enum value
uint8_t code_byte = buf[0];
MessageCode code = (MessageCode)code_byte;
uint32_t size = 0;
uint16_t abs, rel, key, *buf16;
uint16_t abs, rel, key, index, *buf16;
switch (code) {
case DeviceInfo:
@ -20,14 +38,16 @@ int msg_deserialize(const uint8_t *buf, size_t len, Message *restrict dst) {
return -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 += 8;
index = buf16[0];
abs = buf16[1];
rel = buf16[2];
key = buf16[3];
buf += 12;
if (MSS_DEVICE_INFO(abs, rel, key) > len)
return -1;
dst->device_info.code = code;
dst->device_info.index = index;
dst->device_info.abs_count = abs;
dst->device_info.rel_count = rel;
dst->device_info.key_count = key;
@ -57,21 +77,24 @@ int msg_deserialize(const uint8_t *buf, size_t len, Message *restrict dst) {
buf += 2;
}
return 0;
size = MSS_DEVICE_INFO(abs, rel, key) + 1;
break;
case DeviceReport:
if (len < 7)
return -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 += 8;
index = buf16[0];
abs = buf16[1];
rel = buf16[2];
key = buf16[3];
buf += 12;
if (len < MSS_DEVICE_REPORT(abs, rel, key))
return -1;
dst->device_report.code = code;
dst->device_report.index = index;
dst->device_report.abs_count = abs;
dst->device_report.rel_count = rel;
dst->device_report.key_count = key;
@ -89,32 +112,104 @@ int msg_deserialize(const uint8_t *buf, size_t len, Message *restrict dst) {
for (int i = 0; i < key; i++)
dst->device_report.key[i] = *(buf++);
return 0;
buf += align_4(key) - key;
size = MSS_DEVICE_REPORT(abs, rel, key) + 1;
break;
case ControllerState:
if (len < MSS_CONTROLLER_STATE)
return -1;
dst->code = code;
dst->controller_state.led[0] = buf[1];
dst->controller_state.led[1] = buf[2];
dst->controller_state.led[2] = buf[3];
dst->controller_state.small_rumble = buf[4];
dst->controller_state.big_rumble = buf[5];
dst->controller_state.flash_on = buf[6];
dst->controller_state.flash_off = buf[7];
return 0;
dst->controller_state.index = *(uint16_t *)(buf + 2);
dst->controller_state.led[0] = buf[4];
dst->controller_state.led[1] = buf[5];
dst->controller_state.led[2] = buf[6];
dst->controller_state.small_rumble = buf[7];
dst->controller_state.big_rumble = buf[8];
dst->controller_state.flash_on = buf[9];
dst->controller_state.flash_off = buf[10];
size = MSS_CONTROLLER_STATE + 1;
buf += size;
break;
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[str_len] = '\0';
strncpy(str, (char *)buf, str_len);
tags[i] = str;
buf += align_2(str_len);
}
dst->request.requests = tags;
size = expected_len + 1;
break;
}
case DeviceDestroy:
if (len < MSS_DESTROY)
return -1;
dst->code = code;
dst->destroy.index = *(uint16_t *)(buf + 2);
size = MSS_DESTROY + 1;
buf += size;
break;
default:
return -1;
}
if (size + MAGIC_SIZE > len + 1) {
return -1;
}
if (*(MAGIC_TYPE *)buf != MAGIC_END) {
printf("NET: Magic not found\n");
return -1;
}
return size + 2 * MAGIC_SIZE;
}
// 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)
// If len is less than the two magic and the code we can't serialize any message
if (len < MAGIC_SIZE * 2 + 1)
return -1;
*(MAGIC_TYPE *)buf = MAGIC_BEG;
buf += MAGIC_SIZE;
len -= MAGIC_SIZE + 1;
uint16_t abs, rel, key, *buf16;
uint32_t size;
switch (msg->code) {
case DeviceInfo:
@ -129,10 +224,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
buf16 = (uint16_t *)(buf + 2);
// 2 aligned here
buf16[0] = abs;
buf16[1] = rel;
buf16[2] = key;
buf += 8;
buf16[0] = msg->device_info.index;
buf16[1] = abs;
buf16[2] = rel;
buf16[3] = key;
buf += 12;
// Back to 4 aligned
for (int i = 0; i < abs; i++) {
@ -160,7 +256,8 @@ int msg_serialize(uint8_t *restrict buf, size_t len, const Message *msg) {
buf += 2;
}
return MSS_DEVICE_INFO(abs, rel, key) + 1;
size = MSS_DEVICE_INFO(abs, rel, key) + 1;
break;
case DeviceReport:
abs = msg->device_report.abs_count;
rel = msg->device_report.rel_count;
@ -171,10 +268,11 @@ int msg_serialize(uint8_t *restrict buf, size_t len, const Message *msg) {
buf[0] = (uint8_t)msg->code;
// 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 += 8;
buf16[0] = msg->device_report.index;
buf16[1] = abs;
buf16[2] = rel;
buf16[3] = key;
buf += 12;
// We're 4 aligned already
for (int i = 0; i < abs; i++) {
*(uint32_t *)buf = msg->device_report.abs[i];
@ -189,22 +287,114 @@ int msg_serialize(uint8_t *restrict buf, size_t len, const Message *msg) {
for (int i = 0; i < key; i++)
*(buf++) = msg->device_report.key[i];
return MSS_DEVICE_REPORT(abs, rel, key) + 1;
size = MSS_DEVICE_REPORT(abs, rel, key) + 1;
buf += align_4(key) - key;
break;
case ControllerState:
if (len < MSS_CONTROLLER_STATE)
return -1;
buf[0] = (uint8_t)msg->code;
buf[1] = msg->controller_state.led[0];
buf[2] = msg->controller_state.led[1];
buf[3] = msg->controller_state.led[2];
buf[4] = msg->controller_state.small_rumble;
buf[5] = msg->controller_state.big_rumble;
buf[6] = msg->controller_state.flash_on;
buf[7] = msg->controller_state.flash_off;
return MSS_CONTROLLER_STATE + 1;
*(uint16_t *)(buf + 2) = msg->controller_state.index;
buf[4] = msg->controller_state.led[0];
buf[5] = msg->controller_state.led[1];
buf[6] = msg->controller_state.led[2];
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;
size = MSS_CONTROLLER_STATE + 1;
buf += size;
break;
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 + 2;
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;
}
size = expected_len;
break;
}
case DeviceDestroy:
if (len < MSS_DESTROY)
return -1;
buf[0] = (uint8_t)msg->code;
*(uint16_t *)(buf + 2) = msg->controller_state.index;
size = MSS_DESTROY + 1;
buf += size;
break;
default:
printf("ERR(msg_serialize): Trying to serialize unknown message of code %d\n", msg->code);
return -1;
}
if (size + MAGIC_SIZE > len) {
return -1;
}
*(MAGIC_TYPE *)buf = MAGIC_END;
return size + MAGIC_SIZE * 2;
}
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);
}
}
void print_message_buffer(const uint8_t *buf, int len) {
bool last_beg = false;
for (int i = 0; i < len; i++) {
if (i + MAGIC_SIZE <= len) {
MAGIC_TYPE magic = *(MAGIC_TYPE *)(&buf[i]);
if (magic == MAGIC_BEG) {
printf(" \033[32m%08X\033[0m", magic);
i += MAGIC_SIZE - 1;
last_beg = true;
continue;
} else if (magic == MAGIC_END) {
printf(" \033[32m%08X\033[0m", magic);
i += MAGIC_SIZE - 1;
continue;
}
}
if (last_beg) {
last_beg = false;
printf(" \033[034m%02X\033[0m", buf[i]);
} else {
printf(" %02X", buf[i]);
}
}
}

40
net.h
View File

@ -1,15 +1,24 @@
// vi:ft=c
#ifndef NET_H_
#define NET_H_
#include "util.h"
#include <linux/input-event-codes.h>
#include <stdint.h>
#include <stdlib.h>
#define MAGIC_TYPE uint32_t
#define MAGIC_SIZE sizeof(MAGIC_TYPE)
static const MAGIC_TYPE MAGIC_BEG = 0xDEADCAFE;
static const MAGIC_TYPE MAGIC_END = 0xCAFEDEAD;
typedef enum {
NoMessage = 0,
DeviceInfo = 1,
DeviceReport = 2,
DeviceDestroy = 3,
ControllerState = 4,
Request = 5,
} MessageCode;
// Alignment 4
@ -17,6 +26,8 @@ typedef struct {
MessageCode code;
// + 1 byte of padding
uint16_t index;
uint16_t abs_count;
uint16_t rel_count;
uint16_t key_count;
@ -33,7 +44,7 @@ typedef struct {
uint16_t key_id[KEY_CNT];
} 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:
// Size of the data of the message when serialized (no alignment / padding)
@ -41,6 +52,7 @@ typedef struct {
typedef struct {
MessageCode code;
// + 1 byte of padding
uint16_t index;
uint16_t abs_count;
uint16_t rel_count;
@ -50,22 +62,42 @@ 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 + 1)
#define MSS_DEVICE_REPORT(abs, rel, key) (11 + abs * 4 + rel * 4 + align_4(key))
// 1 aligned
typedef struct {
MessageCode code;
// + 1 byte of padding
uint16_t index;
uint8_t led[3];
uint8_t small_rumble;
uint8_t big_rumble;
uint8_t flash_on;
uint8_t flash_off;
} 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 {
MessageCode code;
MessageRequest request;
MessageDestroy destroy;
MessageDeviceInfo device_info;
MessageDeviceReport device_report;
MessageControllerState controller_state;
@ -73,5 +105,7 @@ typedef union {
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);
void msg_free(Message *msg);
void print_message_buffer(const uint8_t *buf, int len);
#endif

445
server.c
View File

@ -1,7 +1,11 @@
#include "server.h"
#include "const.h"
#include "hid.h"
#include "json.h"
#include "net.h"
#include "util.h"
#include "vec.h"
#include <linux/input-event-codes.h>
#include <linux/input.h>
@ -9,6 +13,8 @@
#include <netinet/tcp.h>
#include <poll.h>
#include <pthread.h>
#include <signal.h>
#include <stddef.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
@ -20,8 +26,194 @@
struct Connection {
int socket;
uint32_t id;
bool closed;
};
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[] = {
{".uniq", &StringAdapter, offsetof(ControllerFilter, uniq), default_to_zero_u64, tsf_uniq_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 },
{".name", &StringAdapter, offsetof(ControllerFilter, name), default_to_null, 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")
#define TRAP_IGN(sig) \
if (sigaction(sig, &(struct sigaction){{SIG_IGN}}, NULL) != 0) \
printf("SERVER: can't ignore " #sig ".\n")
void device_thread_exit(int _sig) {
struct DeviceThreadArgs *args = pthread_getspecific(device_args_key);
printf("CONN(%d): [%d] exiting\n", args->conn->id, args->index);
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_IGN(SIGPIPE);
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->conn->closed);
if (ctr == NULL) {
break;
}
*args->controller = ctr;
dev_info = ctr->dev.device_info;
dev_info.index = args->index;
printf("CONN(%d): [%d] Found suitable [%s] device: '%s' (%016lx)\n", args->conn->id, args->index, args->tag,
ctr->dev.name, ctr->dev.id);
// 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;
report.index = args->index;
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(SIGTERM);
return NULL;
}
void *server_handle_conn(void *args_) {
struct Connection *args = args_;
@ -36,154 +228,190 @@ void *server_handle_conn(void *args_) {
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");
uint8_t buf[2048] __attribute__((aligned(4))) = {};
PhysicalDevice dev = get_device();
printf("CONN(%u): got device '%s'\n", args->id, dev.name);
uint8_t buf[2048] __attribute__((aligned(4))) = {0};
char *closing_message = "";
bool got_request = false;
Vec device_threads = vec_of(pthread_t);
Vec device_controllers = vec_of(Controller *);
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;
struct pollfd pfd = {.fd = args->socket, .events = POLLIN};
while (1) {
int rc = poll(pfds, 2, -1);
if (rc < 0) { // error (connection closed)
int rc = poll(&pfd, 1, config.request_timeout);
// 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";
goto conn_end;
}
// Shutdown connection if we lost the peer
if (socket_poll->revents & POLLHUP || socket_poll->revents & POLLERR) {
// Test for error on socket
if (pfd.revents & POLLHUP || pfd.revents & POLLERR) {
closing_message = "Lost peer";
goto conn_end;
}
if (socket_poll->revents & POLLIN) {
// Receive data
int len = recv(args->socket, buf, 2048, 0);
if (len <= 0) {
closing_message = "Lost peer";
closing_message = "Lost peer (from recv)";
goto conn_end;
} else if (len > 1 + MAGIC_SIZE * 2) {
printf("CONN(%d): Got message: ", args->id);
printf("\n");
} else {
printf("CONN(%d): Malformed message\n", args->id);
}
// Parse message
Message msg;
if (msg_deserialize(buf, len, &msg) == 0) {
int msg_len = msg_deserialize(buf, len, &msg);
if (msg_len < 0) {
if (len > 1 + MAGIC_SIZE * 2) {
printf("CONN(%d): Couldn't parse message: ", args->id);
print_message_buffer(buf, len);
printf("\n");
} else {
printf("CONN(%d): Couldn't parse message", args->id);
}
continue;
}
// Handle message
if (msg.code == ControllerState) {
apply_controller_state(&dev, (MessageControllerState *)&msg);
int i = msg.controller_state.index;
if (i >= device_controllers.len) {
printf("CONN(%d): Invalid controller index in controller state message\n", args->id);
continue;
}
Controller *ctr = *(Controller **)vec_get(&device_controllers, i);
if (ctr == NULL) {
printf("CONN(%d): Received controller state message but the device hasn't yet been received\n", args->id);
continue;
}
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);
}
} 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;
}
if (event_poll->revents & POLLIN) {
struct input_event event;
int len = read(dev.event, &event, sizeof(struct input_event));
if (len <= 0) {
closing_message = "Lost device";
goto conn_end;
}
if (len < sizeof(struct input_event)) {
printf("CONN(%d): error reading event\n", args->id);
continue;
}
if (event.type == EV_SYN) {
int len = msg_serialize(buf, 2048, (Message *)&report);
if (len < 0) {
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;
}
}
}
conn_end:
shutdown(args->socket, SHUT_RDWR);
printf("CONN(%u): connection closed (%s)\n", args->id, closing_message);
return_device(&dev);
args->closed = true;
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);
}
free(args);
vec_free(device_threads);
vec_free(device_controllers);
return NULL;
}
void server_run(uint16_t port) {
static int sockfd;
void clean_exit(int _sig) {
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");
// 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);
if (sock < 0) {
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_addr.s_addr = htonl(INADDR_ANY);
addr.sin_port = htons(port);
@ -203,6 +431,7 @@ void server_run(uint16_t port) {
struct Connection conn;
conn.socket = accept(sock, &con_addr, &con_len);
conn.closed = false;
if (conn.socket >= 0) {
printf("SERVER: got connection\n");

30
server.h
View File

@ -1,8 +1,36 @@
// vi:ft=c
#ifndef SERVER_H_
#define SERVER_H_
#include <stdbool.h>
#include <stdint.h>
#include <time.h>
void server_run(uint16_t port);
typedef struct {
// the 0 uniq represents no filter
uint64_t uniq;
// negative values means no filter
int32_t vendor;
// negative values means no filter
int32_t product;
bool js;
// NULL means no filter
char *name;
} 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

145
util.c
View File

@ -1,9 +1,12 @@
#include "util.h"
#include <math.h>
#include <stdarg.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#ifndef __has_builtin
#define __has_builtin(_) 0
@ -34,3 +37,145 @@ uint8_t parse_hex_digit(char h) {
else
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_uniq_to_u64(void *arg, void *ptr) {
char *s = *(char **)arg;
if (strnlen(s, 18) != 17) {
printf("JSON: wrong length for uniq, 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 uniq 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;
}

23
util.h
View File

@ -13,6 +13,29 @@ uint16_t parse_port(const char *str);
static inline bool bit_set(uint8_t *bits, int i) { return bits[i / 8] & (1 << (i % 8)); }
// Align n to the next 8 boundary
static inline size_t align_8(size_t n) { return (((n - 1) >> 3) + 1) << 3; }
// Align n to the next 4 boundary
static inline size_t align_4(size_t n) { return (((n - 1) >> 2) + 1) << 2; }
// Align n to the next 2 boundary
static inline size_t align_2(size_t n) { return (((n - 1) >> 1) + 1) << 1; }
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_uniq_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

11
vec.c
View File

@ -1,12 +1,13 @@
#include "vec.h"
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#define INIT_CAP 8
static void handle_alloc_error() {
static void handle_alloc_error(void) {
printf("Error when allocating memory.\n");
exit(2);
}
@ -40,7 +41,7 @@ static inline void vec_grow(Vec *v, size_t cap) {
void vec_push(Vec *v, void *data) {
vec_grow(v, v->len + 1);
memcpy(v->data + v->stride * v->len++, data, v->stride);
memcpy((uint8_t *)v->data + v->stride * v->len++, data, v->stride);
}
void vec_pop(Vec *v, void *data) {
@ -68,7 +69,7 @@ void vec_insert(Vec *v, void *data, size_t index) {
}
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) {
memmove(slot + v->stride, slot, (v->len - index) * v->stride);
}
@ -86,7 +87,7 @@ void vec_remove(Vec *v, size_t index, void *data) {
return;
}
void *slot = v->data + index * v->stride;
uint8_t *slot = v->data + index * v->stride;
if (data != NULL) {
memcpy(data, slot, v->stride);
}
@ -103,7 +104,7 @@ void vec_extend(Vec *v, void *data, size_t len) {
return;
}
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;
}

3
vec.h
View File

@ -1,12 +1,13 @@
// vi:ft=c
#ifndef VEC_H_
#define VEC_H_
#include <stdint.h>
#include <unistd.h>
#define vec_of(type) vec_new(sizeof(type))
typedef struct {
void *data;
uint8_t *data;
size_t cap;
size_t len;
size_t stride;