stupidcomputer/jsfw
1
0
Fork 0
mirror of https://github.com/stupidcomputer/jsfw.git synced 2024-12-26 21:42:23 -06:00
Code Issues Packages Projects Releases Wiki Activity

cleanup, small reorganisation, never gonna run around and desert you

Browse Source
This commit is contained in:
viandoxdev 2022-08-31 23:24:26 +02:00
parent fc6169f954
commit 7220bdd517
No known key found for this signature in database
GPG Key ID: AF1410C5BC10AA25
16 changed files with 534 additions and 270 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Makefile
View File

@ -1,6 +1,6 @@
Q=@ Q=@
CC=gcc CC=gcc
CFLAGS=-g -Wall -Wno-format-truncation -pthread -DJSFW_DEV -lm CFLAGS=-g -Wall -Wno-format-truncation -pthread -lm
LDFLAGS= LDFLAGS=
BUILD_DIR=./objects BUILD_DIR=./objects
BIN=jsfw BIN=jsfw

6
README.md
View File

@ -1,6 +1,6 @@
# jsfw # jsfw
Linux only utility to forward uevent devices over network through a tcp connection. Utility to forward uevent devices over network through a tcp connection.
# Usage # Usage
@ -18,7 +18,7 @@ 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. 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 (either `/tmp/jsfw_fifo` or `/run/jsfw_fifo` depending on if `JSFW_DEV` was set during compilation, see `Makefile`). 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): The format for the controller state takes this form (comments not allowed):
@ -32,6 +32,8 @@ The format for the controller state takes this form (comments not allowed):
Any value can be ommitted, extra values will be ignored. Any value can be ommitted, extra values will be ignored.
Some aspect are easily configurable through `const.c`.
# Building # Building
## Dependencies ## Dependencies

96
client.c
View File

@ -1,5 +1,6 @@
#include "client.h" #include "client.h"
#include "const.h"
#include "json.h" #include "json.h"
#include "net.h" #include "net.h"
#include "util.h" #include "util.h"
@ -24,24 +25,13 @@
#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 { typedef struct {
int fd; int fd;
MessageDeviceInfo info; MessageDeviceInfo info;
} VirtualDevice; } VirtualDevice;
#ifdef JSFW_DEV
// Path for dev environment (no root)
const char *FIFO_PATH = "/tmp/jsfw_fifo";
#else
const char *FIFO_PATH = "/run/jsfw_fifo";
#endif
const int CONN_RETRY_DELAY = 5;
// Constant for the virtual device
const uint16_t VIRT_VENDOR = 0x6969;
const uint16_t VIRT_PRODUCT = 0x0420;
const uint16_t VIRT_VERSION = 1;
const char *VIRT_NAME = "JSFW Virtual Device";
static int fifo_attempt = 0; static int fifo_attempt = 0;
static struct sockaddr_in server_addr = {}; static struct sockaddr_in server_addr = {};
@ -57,8 +47,10 @@ static int sock = -1;
static Message message; static Message message;
static VirtualDevice device = {}; static VirtualDevice device = {};
// Test if the device exists
static inline bool device_exists() { return device.fd > 0; } static inline bool device_exists() { return device.fd > 0; }
// Struct representing the received json
typedef struct { typedef struct {
char *led_color; char *led_color;
double rumble_small; double rumble_small;
@ -75,6 +67,7 @@ static const JSONAdapter JControllerStateAdapter[] = {
{".flash.1", Number, offsetof(JControllerState, flash_off)}, {".flash.1", Number, offsetof(JControllerState, flash_off)},
}; };
// Try to destroy the device
void device_destroy() { void device_destroy() {
if (!device_exists()) { if (!device_exists()) {
return; return;
@ -86,6 +79,7 @@ void device_destroy() {
printf("CLIENT: Destroyed device\n"); printf("CLIENT: Destroyed device\n");
} }
// (Re)Initialize the device
void device_init(MessageDeviceInfo *dev) { void device_init(MessageDeviceInfo *dev) {
device_destroy(); device_destroy();
@ -95,6 +89,8 @@ void device_init(MessageDeviceInfo *dev) {
exit(1); exit(1);
} }
// Setup device_info
// 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);
@ -130,10 +126,10 @@ void device_init(MessageDeviceInfo *dev) {
struct uinput_setup setup = {}; struct uinput_setup setup = {};
setup.id.bustype = BUS_VIRTUAL; setup.id.bustype = BUS_VIRTUAL;
setup.id.vendor = VIRT_VENDOR; setup.id.vendor = VIRTUAL_DEVICE_VENDOR;
setup.id.product = VIRT_PRODUCT; setup.id.product = VIRTUAL_DEVICE_PRODUCT;
setup.id.version = VIRT_VERSION; setup.id.version = VIRTUAL_DEVICE_VERSION;
strncpy(setup.name, VIRT_NAME, UINPUT_MAX_NAME_SIZE); strncpy(setup.name, VIRTUAL_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);
@ -144,6 +140,7 @@ void device_init(MessageDeviceInfo *dev) {
dev->key_count); dev->key_count);
} }
// Send an event to uinput, device must exist
int device_emit(uint16_t type, uint16_t id, uint32_t value) { int device_emit(uint16_t type, uint16_t id, uint32_t value) {
struct input_event event = {}; struct input_event event = {};
@ -154,9 +151,10 @@ int device_emit(uint16_t type, uint16_t id, uint32_t value) {
return write(device.fd, &event, sizeof(event)) != sizeof(event); return write(device.fd, &event, sizeof(event)) != sizeof(event);
} }
// Update device with report
void device_handle_report(MessageDeviceReport *report) { void device_handle_report(MessageDeviceReport *report) {
if (!device_exists()) { if (!device_exists()) {
printf("CLIENT: Got report but device info\n"); printf("CLIENT: Got report before device info\n");
return; return;
} }
@ -167,25 +165,30 @@ void device_handle_report(MessageDeviceReport *report) {
} }
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(EV_ABS, device.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(EV_REL, device.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(EV_KEY, device.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
// send one when we receive the report to match
device_emit(EV_SYN, 0, 0); device_emit(EV_SYN, 0, 0);
} }
void setup_fifo(); void setup_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(FIFO_PATH, O_RDONLY | O_NONBLOCK);
@ -196,8 +199,10 @@ void open_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");
} }
fifo_attempt = 0;
} }
// 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(FIFO_PATH, 0666);
@ -209,27 +214,31 @@ void setup_fifo() {
fifo_poll->events = POLLIN; fifo_poll->events = POLLIN;
} }
// (Re)Connect to the server
void connect_server() { void connect_server() {
while (1) { while (1) {
if (sock > 0) { if (sock > 0) {
// Close previous connection
device_destroy(); device_destroy();
shutdown(sock, SHUT_RDWR); shutdown(sock, SHUT_RDWR);
close(sock); close(sock);
} }
sock = socket(AF_INET, SOCK_STREAM, 0); sock = socket(AF_INET, SOCK_STREAM, 0);
if (sock < 0) if (sock < 0) {
panicf("Couldn't create socket\n"); panicf("Couldn't create socket\n");
}
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 %ds\n", server_addrp, server_port,
CONN_RETRY_DELAY); CONNECTION_RETRY_DELAY);
struct timespec ts = {}; struct timespec ts = {};
ts.tv_sec = CONN_RETRY_DELAY; ts.tv_sec = CONNECTION_RETRY_DELAY;
nanosleep(&ts, NULL); nanosleep(&ts, NULL);
continue; continue;
} }
// Set non blocking // Set non blocking, only do that after connection (instead of with SOCK_NONBLOCK at socket creation)
// because we want to block on the connection itself
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");
@ -237,12 +246,16 @@ void connect_server() {
} }
} }
// Setup server address and connects to it (+ setup poll_fd)
void setup_server(char *address, uint16_t port) { void setup_server(char *address, uint16_t port) {
// setup address // setup address
server_addr.sin_family = AF_INET; server_addr.sin_family = AF_INET;
if (inet_pton(AF_INET, address, &server_addr.sin_addr) == 0)
if (inet_pton(AF_INET, address, &server_addr.sin_addr) == 0) {
printf("CLIENT: failed to parse address '%s', defaulting to 0.0.0.0 (localhost)\n", address); printf("CLIENT: failed to parse address '%s', defaulting to 0.0.0.0 (localhost)\n", address);
}
inet_ntop(AF_INET, &server_addr.sin_addr, server_addrp, 64); inet_ntop(AF_INET, &server_addr.sin_addr, server_addrp, 64);
server_port = port; server_port = port;
server_addr.sin_port = htons(port); server_addr.sin_port = htons(port);
@ -260,17 +273,6 @@ void early_checks() {
close(fd); close(fd);
} }
static uint8_t parse_hex_digit(char h) {
if (h >= '0' && h <= '9')
return h - '0';
else if (h >= 'a' && h <= 'f')
return h - 'a' + 10;
else if (h >= 'A' && h <= 'F')
return h - 'A' + 10;
else
return 0;
}
void client_run(char *address, uint16_t port) { void client_run(char *address, uint16_t port) {
// Device doesn't exist yet // Device doesn't exist yet
device.fd = -1; device.fd = -1;
@ -281,6 +283,7 @@ void client_run(char *address, uint16_t port) {
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)));
while (1) { while (1) {
int rc = poll(poll_fds, 2, -1); int rc = poll(poll_fds, 2, -1);
if (rc < 0) { if (rc < 0) {
@ -297,7 +300,7 @@ void client_run(char *address, uint16_t port) {
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_err_loc()); json_strerr(), json_errloc());
} else { } else {
JControllerState state; JControllerState state;
// default values // default values
@ -306,8 +309,10 @@ void client_run(char *address, uint16_t port) {
state.led_color = NULL; state.led_color = NULL;
state.rumble_small = 0.0; state.rumble_small = 0.0;
state.rumble_big = 0.0; state.rumble_big = 0.0;
json_adapt(json_buf, (JSONAdapter *)JControllerStateAdapter, json_adapt(json_buf, (JSONAdapter *)JControllerStateAdapter,
sizeof(JControllerStateAdapter) / sizeof(JSONAdapter), &state); 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); msg.small_rumble = (uint8_t)(fmax(fmin(1.0, state.rumble_small), 0.0) * 255.0);
@ -357,34 +362,35 @@ void client_run(char *address, uint16_t port) {
shutdown(sock, SHUT_RDWR); 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 continue here because there's nothing after, unlike above for fifo (this reduces
// indentation) // indentation instead of needing an else block)
continue; continue;
} }
// We've got data from the server // We've got data from the server
if (msg_deserialize(buf, len, &message) != 0) { if (msg_deserialize(buf, len, &message) != 0) {
printf("CLIENT: Couldn't parse message (code: %d, len: %d)\n", buf[0], len); printf("CLIENT: Couldn't parse message (code: %d, len: %d)\n", buf[0], len);
int l = len > 100 ? 100 : len; int l = len > 100 ? 100 : len;
for (int i = 0; i < l; i++) { for (int i = 0; i < l; i++) {
printf("%02x", buf[i]); printf("%02x", buf[i]);
} }
if (len > 100) { if (len > 100) {
printf(" ... %d more bytes", len - 100); printf(" ... %d more bytes", len - 100);
} }
printf("\n"); printf("\n");
continue; continue;
} }
if (message.code == DeviceInfo) { if (message.code == DeviceInfo) {
if (device_exists()) {
if (device_exists())
printf("CLIENT: Got more than one device info\n"); printf("CLIENT: Got more than one device info\n");
}
device_init((MessageDeviceInfo *)&message); device_init((MessageDeviceInfo *)&message);
} else if (message.code == DeviceReport) { } else if (message.code == DeviceReport) {
device_handle_report((MessageDeviceReport *)&message); device_handle_report((MessageDeviceReport *)&message);
} else { } else {
printf("CLIENT: Illegal message\n"); printf("CLIENT: Illegal message\n");
} }

29
const.c Normal file
View File

@ -0,0 +1,29 @@
#include "const.h"
#include <stdint.h>
#include <time.h>
// How long between each device poll
const struct timespec POLL_DEVICE_INTERVAL = {.tv_sec = 1, .tv_nsec = 0};
// 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;
// Displayed bendor for the virtual device
const uint16_t VIRTUAL_DEVICE_VENDOR = 0x6969;
// Displayed product for the virtual device
const uint16_t VIRTUAL_DEVICE_PRODUCT = 0x0420;
// Displayed version for the virtual device
const uint16_t VIRTUAL_DEVICE_VERSION = 1;
// Displayed name for the virtual device
const char *VIRTUAL_DEVICE_NAME = "JSFW Virtual Device";
// Wether to enable keepalive on the tcp spcket
const int TCP_KEEPALIVE_ENABLE = 1;
// How much idle time before sending probe packets
const int TCP_KEEPALIVE_IDLE_TIME = 10;
// How many probes before giving up
const int TCP_KEEPALIVE_RETRY_COUNT = 5;
// How long (in seconds) between each probes
const int TCP_KEEPALIVE_RETRY_INTERVAL = 2;

20
const.h Normal file
View File

@ -0,0 +1,20 @@
// vi:ft=c
#ifndef CONST_H_
#define CONST_H_
#include <stdint.h>
#include <time.h>
extern const struct timespec POLL_DEVICE_INTERVAL;
extern const char *DEVICE_DEFAULT_NAME;
extern const char *FIFO_PATH;
extern const uint32_t CONNECTION_RETRY_DELAY;
extern const uint16_t VIRTUAL_DEVICE_VENDOR;
extern const uint16_t VIRTUAL_DEVICE_PRODUCT;
extern const uint16_t VIRTUAL_DEVICE_VERSION;
extern const char *VIRTUAL_DEVICE_NAME;
extern const int TCP_KEEPALIVE_ENABLE;
extern const int TCP_KEEPALIVE_IDLE_TIME;
extern const int TCP_KEEPALIVE_RETRY_COUNT;
extern const int TCP_KEEPALIVE_RETRY_INTERVAL;
#endif

147
hid.c
View File

@ -1,5 +1,7 @@
#include "hid.h" #include "hid.h"
#include "const.h"
#include "util.h"
#include "vec.h" #include "vec.h"
#include <dirent.h> #include <dirent.h>
@ -28,8 +30,6 @@ 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;
static char *DEFAULT_NAME = "Unnamed Device";
// 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]) {
uniq_t res = 0; uniq_t res = 0;
@ -50,8 +50,7 @@ uniq_t parse_uniq(char uniq[17]) {
return res; return res;
} }
static inline bool bit_set(uint8_t *bits, int i) { return bits[i / 8] & (1 << (i % 8)); } // Finish setup of a partially initialized device (set device_info and mapping)
void setup_device(PhysicalDevice *dev) { void setup_device(PhysicalDevice *dev) {
dev->device_info.code = DeviceInfo; dev->device_info.code = DeviceInfo;
dev->device_info.abs_count = 0; dev->device_info.abs_count = 0;
@ -65,42 +64,61 @@ 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 bits[EV_MAX] = {}; uint8_t type_bits[EV_MAX] = {};
uint8_t feat_bits[(KEY_MAX + 7) / 8] = {}; uint8_t feat_bits[(KEY_MAX + 7) / 8] = {};
ioctl(dev->event, EVIOCGBIT(0, EV_MAX), bits);
for (int i = 0; i < EV_MAX; i++) { ioctl(dev->event, EVIOCGBIT(0, EV_MAX), type_bits);
if (bit_set(bits, i)) { // Loop over all event types
for (int type = 0; type < EV_MAX; type++) {
// Ignore if the the device doesn't have this even type
if (!bit_set(type_bits, type)) {
continue;
}
memset(feat_bits, 0, sizeof(feat_bits)); memset(feat_bits, 0, sizeof(feat_bits));
ioctl(dev->event, EVIOCGBIT(i, KEY_MAX), feat_bits); ioctl(dev->event, EVIOCGBIT(type, KEY_MAX), feat_bits);
for (int j = 0; j < KEY_MAX; j++) {
if (bit_set(feat_bits, j)) { // Loop over "instances" of type (i.e Each axis of a controller for EV_ABS)
if (i == EV_ABS) { for (int i = 0; i < KEY_MAX; i++) {
if (!bit_set(feat_bits, i)) {
continue;
}
if (type == EV_ABS) {
struct input_absinfo abs; struct input_absinfo abs;
ioctl(dev->event, EVIOCGABS(j), &abs); ioctl(dev->event, EVIOCGABS(i), &abs);
uint16_t index = dev->device_info.abs_count++; uint16_t index = dev->device_info.abs_count++;
dev->device_info.abs_id[index] = j;
dev->device_info.abs_id[index] = i;
dev->device_info.abs_min[index] = abs.minimum; dev->device_info.abs_min[index] = abs.minimum;
dev->device_info.abs_max[index] = abs.maximum; dev->device_info.abs_max[index] = abs.maximum;
dev->device_info.abs_fuzz[index] = abs.fuzz; dev->device_info.abs_fuzz[index] = abs.fuzz;
dev->device_info.abs_flat[index] = abs.flat; dev->device_info.abs_flat[index] = abs.flat;
dev->device_info.abs_res[index] = abs.resolution; dev->device_info.abs_res[index] = abs.resolution;
dev->mapping.abs_indices[j] = index; dev->mapping.abs_indices[i] = index;
} else if (i == EV_REL) { } else if (type == EV_REL) {
uint16_t index = dev->device_info.rel_count++; uint16_t index = dev->device_info.rel_count++;
dev->device_info.rel_id[index] = j;
dev->mapping.rel_indices[j] = index; dev->device_info.rel_id[index] = i;
} else if (i == EV_KEY) { dev->mapping.rel_indices[i] = index;
} else if (type == EV_KEY) {
uint16_t index = dev->device_info.key_count++; uint16_t index = dev->device_info.key_count++;
dev->device_info.key_id[index] = j;
dev->mapping.key_indices[j] = index; dev->device_info.key_id[index] = i;
} dev->mapping.key_indices[i] = index;
}
} }
} }
} }
} }
// Function used to filter out devices that we don't want.
// This is pretty arbritrary
bool filter_event(int fd, char *event) { 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);
@ -120,19 +138,24 @@ bool filter_event(int fd, char *event) {
if (!found) { if (!found) {
return false; return false;
} }
}
// Check product and vendor id 054c:05c4 => Dualshock 4
uint16_t info[4]; uint16_t info[4];
ioctl(fd, EVIOCGID, info); ioctl(fd, EVIOCGID, info);
return info[1] == 0x054c && info[2] == 0x05c4; return info[1] == 0x054c && info[2] == 0x05c4;
} }
// Initialize vectors for polling
void poll_devices_init() { void poll_devices_init() {
devices = vec_of(uniq_t); devices = vec_of(uniq_t);
new_devices = vec_of(PhysicalDevice); new_devices = vec_of(PhysicalDevice);
devices_queue = vec_of(PhysicalDevice); devices_queue = vec_of(PhysicalDevice);
} }
// Block to get a device, this is thread safe
PhysicalDevice get_device() { PhysicalDevice get_device() {
// Check if we can get one right away
pthread_mutex_lock(&devices_queue_mutex); pthread_mutex_lock(&devices_queue_mutex);
if (devices_queue.len > 0) { if (devices_queue.len > 0) {
PhysicalDevice r; PhysicalDevice r;
@ -141,28 +164,43 @@ PhysicalDevice get_device() {
return r; return r;
} }
// Wait on condvar until there's a device and we can unlock the mutex
while (devices_queue.len == 0) { while (devices_queue.len == 0) {
pthread_cond_wait(&devices_queue_cond, &devices_queue_mutex); pthread_cond_wait(&devices_queue_cond, &devices_queue_mutex);
} }
// Take a device from the queue
PhysicalDevice res; PhysicalDevice res;
vec_pop(&devices_queue, &res); vec_pop(&devices_queue, &res);
// Signal another thread if there are still device(s) left in the queue
if (devices_queue.len > 0) { if (devices_queue.len > 0) {
pthread_cond_signal(&devices_queue_cond); pthread_cond_signal(&devices_queue_cond);
} }
pthread_mutex_unlock(&devices_queue_mutex); pthread_mutex_unlock(&devices_queue_mutex);
return res; 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) { void return_device(PhysicalDevice *dev) {
if (dev->name != NULL && dev->name != DEFAULT_NAME) { 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); printf("HID: Returning device '%s' (%012lx)\n", dev->name, dev->uniq);
free(dev->name); free(dev->name);
} else { } else {
printf("HID: Returning device %012lx\n", dev->uniq); 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->event);
close(dev->hidraw); close(dev->hidraw);
// Safely remove device from the known device list
pthread_mutex_lock(&devices_mutex); pthread_mutex_lock(&devices_mutex);
for (int i = 0; i < devices.len; i++) { for (int i = 0; i < devices.len; i++) {
uniq_t *uniq = vec_get(&devices, i); uniq_t *uniq = vec_get(&devices, i);
@ -174,38 +212,44 @@ void return_device(PhysicalDevice *dev) {
pthread_mutex_unlock(&devices_mutex); pthread_mutex_unlock(&devices_mutex);
} }
// Find all available devices and pick up on new ones
void poll_devices() { void poll_devices() {
vec_clear(&new_devices); vec_clear(&new_devices);
// 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 linkg 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) {
continue; continue;
} }
PhysicalDevice dev; PhysicalDevice dev;
// Open /dev/input/eventXX
char event_path[64]; char event_path[64];
snprintf(event_path, 64, "/dev/input/%s", input->d_name); 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) { if (dev.event < 0) { // Ignore device if we couldn't open
continue; continue;
} }
// Try to get the name, default to DEFAULT_NAME if impossible
char name_buf[256] = {}; char name_buf[256] = {};
char *name; const char *name;
if (ioctl(dev.event, EVIOCGNAME(256), name_buf) >= 0) if (ioctl(dev.event, EVIOCGNAME(256), name_buf) >= 0)
name = name_buf; name = name_buf;
else else
name = DEFAULT_NAME; name = DEVICE_DEFAULT_NAME;
// Filter events we don't care about
if (!filter_event(dev.event, input->d_name)) if (!filter_event(dev.event, input->d_name))
goto skip; goto skip;
// Try to get uniq, drop device if we can't
uniq_t uniq; uniq_t uniq;
{ {
char uniq_str[17] = {}; char uniq_str[17] = {};
@ -218,6 +262,7 @@ void poll_devices() {
goto skip; goto skip;
} }
// Check if we already know of this device
bool found = false; bool found = false;
pthread_mutex_lock(&devices_mutex); pthread_mutex_lock(&devices_mutex);
@ -230,13 +275,14 @@ void poll_devices() {
} }
pthread_mutex_unlock(&devices_mutex); pthread_mutex_unlock(&devices_mutex);
if (found) if (found) { // Device isn't new
goto skip; goto skip;
}
dev.uniq = uniq; dev.uniq = uniq;
// Try to find hidraw path for the device, drop the device if we can't
char hidraw_path[64]; char hidraw_path[64];
{ {
char hidraw_dir_path[256]; char hidraw_dir_path[256];
snprintf(hidraw_dir_path, 256, "/sys/class/input/%s/device/device/hidraw", input->d_name); snprintf(hidraw_dir_path, 256, "/sys/class/input/%s/device/device/hidraw", input->d_name);
@ -244,9 +290,10 @@ void poll_devices() {
DIR *hidraw_dir = opendir(hidraw_dir_path); DIR *hidraw_dir = opendir(hidraw_dir_path);
struct dirent *hidraw = NULL; struct dirent *hidraw = NULL;
while ((hidraw = readdir(hidraw_dir)) != NULL) { while ((hidraw = readdir(hidraw_dir)) != NULL) {
if (strncmp(hidraw->d_name, "hidraw", 6) == 0) if (strncmp(hidraw->d_name, "hidraw", 6) == 0) {
break; break;
} }
}
if (hidraw == NULL) { if (hidraw == NULL) {
printf("Couldn't get hidraw of %s", input->d_name); printf("Couldn't get hidraw of %s", input->d_name);
@ -259,23 +306,30 @@ void poll_devices() {
} }
dev.hidraw = open(hidraw_path, O_WRONLY); dev.hidraw = open(hidraw_path, O_WRONLY);
if (dev.hidraw < 0) if (dev.hidraw < 0) {
goto skip; goto skip;
}
if (name != DEVICE_DEFAULT_NAME) {
dev.name = malloc(256); dev.name = malloc(256);
if (dev.name == NULL)
dev.name = DEFAULT_NAME; if (dev.name == NULL) {
else dev.name = (char *)DEVICE_DEFAULT_NAME;
} else {
strcpy(dev.name, name); strcpy(dev.name, name);
}
}
setup_device(&dev); setup_device(&dev);
pthread_mutex_lock(&devices_mutex); pthread_mutex_lock(&devices_mutex);
vec_push(&devices, &uniq); vec_push(&devices, &uniq);
pthread_mutex_unlock(&devices_mutex); pthread_mutex_unlock(&devices_mutex);
vec_push(&new_devices, &dev); vec_push(&new_devices, &dev);
printf("HID: New device, %s (%s: %012lx)\n", name, input->d_name, dev.uniq); printf("HID: New device, %s (%s: %012lx)\n", name, input->d_name, dev.uniq);
// Continue here to avoid running cleanup code of skip
continue; continue;
// close open file descriptor and continue // close open file descriptor and continue
@ -283,8 +337,9 @@ void poll_devices() {
close(dev.event); close(dev.event);
continue; continue;
}; };
closedir(input_dir); closedir(input_dir);
// Safely add new devices to the queue
if (new_devices.len > 0) { if (new_devices.len > 0) {
pthread_mutex_lock(&devices_queue_mutex); pthread_mutex_lock(&devices_queue_mutex);
vec_extend(&devices_queue, new_devices.data, new_devices.len); vec_extend(&devices_queue, new_devices.data, new_devices.len);
@ -294,10 +349,12 @@ void poll_devices() {
} }
} }
// "Execute" a MessageControllerState: set the led color and such using the hidraw interface
void apply_controller_state(PhysicalDevice *dev, MessageControllerState *state) { void apply_controller_state(PhysicalDevice *dev, MessageControllerState *state) {
printf("HID: Controller state: #%02x%02x%02x (%d, %d) rumble: (%d, %d)\n", state->led[0], printf("HID: (%012lx) Controller state: #%02x%02x%02x (%d, %d) rumble: (%d, %d)\n", dev->uniq,
state->led[1], state->led[2], state->flash_on, state->flash_off, state->small_rumble, state->led[0], state->led[1], state->led[2], state->flash_on, state->flash_off,
state->big_rumble); state->small_rumble, state->big_rumble);
uint8_t buf[32] = {0x05, 0xff, 0x00, 0x00}; uint8_t buf[32] = {0x05, 0xff, 0x00, 0x00};
buf[4] = state->small_rumble; buf[4] = state->small_rumble;
@ -310,21 +367,23 @@ void apply_controller_state(PhysicalDevice *dev, MessageControllerState *state)
write(dev->hidraw, buf, 32); write(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
fsync(dev->hidraw); fsync(dev->hidraw);
// Send a second time because it doesn't work otherwise // Send a second time, for some reason the flash doesn't stop otherwise
write(dev->hidraw, buf, 32); write(dev->hidraw, buf, 32);
}; };
} }
// Body of the hid thread
void *hid_thread() { void *hid_thread() {
printf("HID: start\n"); printf("HID: start\n");
poll_devices_init(); poll_devices_init();
while (1) { while (1) {
poll_devices(); poll_devices();
struct timespec ts;
ts.tv_sec = 1; nanosleep(&POLL_DEVICE_INTERVAL, NULL);
ts.tv_nsec = 0;
nanosleep(&ts, NULL);
} }
return NULL; return NULL;
} }

6
hid.h
View File

@ -6,14 +6,20 @@
#include <linux/input-event-codes.h> #include <linux/input-event-codes.h>
#include <stdint.h> #include <stdint.h>
// Unique identifier for devices (provided by linux), May be the mac address
typedef uint64_t uniq_t; typedef uint64_t uniq_t;
// Mapping to go from index to id of events
// the id of an event is the code field of a input_event struct
// the index is given (somewhat arbitrarily) by hid.c::setup_device, this is done because ids are sparse
// and innefficient to transfer over network (especially for keys that can range from 0 to 700).
typedef struct { typedef struct {
uint16_t abs_indices[ABS_CNT]; uint16_t abs_indices[ABS_CNT];
uint16_t rel_indices[REL_CNT]; uint16_t rel_indices[REL_CNT];
uint16_t key_indices[KEY_CNT]; uint16_t key_indices[KEY_CNT];
} DeviceMap; } DeviceMap;
// A struct representing a connected device
typedef struct { typedef struct {
int event; int event;
int hidraw; int hidraw;

307
json.c
View File

@ -1,6 +1,8 @@
#define JSON_C_ #define JSON_C_
#include "json.h" #include "json.h"
#include "util.h"
#include <math.h> #include <math.h>
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h> #include <stdint.h>
@ -8,38 +10,58 @@
#include <stdlib.h> #include <stdlib.h>
#include <string.h> #include <string.h>
// Code for the last json parsing error
static JSONError jerrno = NoError; static JSONError jerrno = NoError;
// Location of the last json parsing error
static size_t jerr_index = 0; 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() { return JSONErrorMessage[jerrno]; }
// Get the code of the last json parsing error
size_t json_err_loc() { return jerr_index; } JSONError json_errno() { return jerrno; }
// Get the location of the last json parsing error
size_t json_errloc() { return jerr_index; }
// Shorthand to set jerno and return -1; // Shorthand to set jerno and return -1;
// i.e
// ```c
// if(error) return set_jerrno(JSONError);
// ```
static inline int set_jerrno(JSONError err) { static inline int set_jerrno(JSONError err) {
jerrno = err; jerrno = err;
return -1; return -1;
} }
static inline size_t align_8(size_t n) { return (((n - 1) >> 3) + 1) << 3; }
// Return true if c is a whitespace character
static inline bool is_whitespace(char c) { return c == ' ' || c == '\t' || c == '\n'; } static inline bool is_whitespace(char c) { return c == ' ' || c == '\t' || c == '\n'; }
static inline void skip_whitespaces(const char **buf, const char *buf_end) {
while (*buf < buf_end && is_whitespace(**buf)) {
(*buf)++;
}
}
static int json_parse_value(const char **buf, const char *buf_end, uint8_t **restrict dst, static int json_parse_value(const char **buf, const char *buf_end, uint8_t **restrict dst,
const uint8_t *dst_end); const uint8_t *dst_end); // Declaration for recursion
// *dst must be 8 aligned // *dst must be 8 aligned
static inline int json_parse_string(const char **buf, const char *buf_end, uint8_t **restrict dst, static inline int json_parse_string(const char **buf, const char *buf_end, uint8_t **restrict dst,
const uint8_t *dst_end) { const uint8_t *dst_end) {
// Ensure enough space for the header
if (*dst + sizeof(JSONHeader) >= dst_end) { if (*dst + sizeof(JSONHeader) >= dst_end) {
return set_jerrno(DstOverflow); return set_jerrno(DstOverflow);
} }
// Build header
JSONHeader *header = (JSONHeader *)(*dst); JSONHeader *header = (JSONHeader *)(*dst);
header->type = (uint32_t)String; header->type = (uint32_t)String;
header->len = 0; header->len = 0;
// Increment dst pointer
*dst += sizeof(JSONHeader); *dst += sizeof(JSONHeader);
// Skip first quote // Skip first quote
(*buf)++; (*buf)++;
// Ensure there is more in the buffer (there should be at least a closing ")
if (*buf == buf_end) { if (*buf == buf_end) {
return set_jerrno(SrcOverflow); return set_jerrno(SrcOverflow);
} }
@ -52,10 +74,12 @@ static inline int json_parse_string(const char **buf, const char *buf_end, uint8
// The unicode codepoint we're parsing // The unicode codepoint we're parsing
int un_codepoint = 0; int un_codepoint = 0;
// Loop until return or we met the end of the buffer
for (; *buf < buf_end; (*buf)++) { for (; *buf < buf_end; (*buf)++) {
char c = **buf; char c = **buf;
if (esc_unicode >= 0) { if (esc_unicode >= 0) { // We're currently in a \uXXXX escape
// Parse hex digit
int digit = 0; int digit = 0;
if (c >= '0' && c <= '9') if (c >= '0' && c <= '9')
digit = c - '0'; digit = c - '0';
@ -66,44 +90,53 @@ static inline int json_parse_string(const char **buf, const char *buf_end, uint8
else { else {
return set_jerrno(StringBadUnicode); return set_jerrno(StringBadUnicode);
} }
un_codepoint <<= 4; un_codepoint <<= 4;
un_codepoint += digit; un_codepoint += digit;
esc_unicode++; esc_unicode++;
if (esc_unicode == 4) { // UTF-8 Encoding // If we got all 4 hex digit, we UTF-8 encode the resulting codepoint
if (un_codepoint <= 0x7f) { if (esc_unicode == 4) {
if (*dst + 1 >= dst_end) // From https://en.wikipedia.org/wiki/UTF-8#Encoding
if (un_codepoint <= 0x7f) { // 1 byte codepoint => ascii
if (*dst + 1 >= dst_end) { // Ensure enough space in the dst buffer
return set_jerrno(DstOverflow); return set_jerrno(DstOverflow);
}
// *(*dst)++ => set **dst to RHS and increment *dst
*(*dst)++ = un_codepoint; *(*dst)++ = un_codepoint;
header->len++; header->len++;
} else if (un_codepoint <= 0x7ff) { } else if (un_codepoint <= 0x7ff) { // 2 byte codepoint
if (*dst + 2 >= dst_end) if (*dst + 2 >= dst_end) {
return set_jerrno(DstOverflow); return set_jerrno(DstOverflow);
}
*(*dst)++ = 0b11000000 | (un_codepoint >> 6 & 0b011111); *(*dst)++ = 0b11000000 | (un_codepoint >> 6 & 0b011111);
*(*dst)++ = 0b10000000 | (un_codepoint >> 0 & 0b111111); *(*dst)++ = 0b10000000 | (un_codepoint >> 0 & 0b111111);
header->len += 2; header->len += 2;
} else if (un_codepoint <= 0xffff) { } 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)++ = 0b11100000 | (un_codepoint >> 12 & 0b1111);
*(*dst)++ = 0b10000000 | (un_codepoint >> 6 & 0b111111); *(*dst)++ = 0b10000000 | (un_codepoint >> 6 & 0b111111);
*(*dst)++ = 0b10000000 | (un_codepoint >> 0 & 0b111111); *(*dst)++ = 0b10000000 | (un_codepoint >> 0 & 0b111111);
header->len += 3; header->len += 3;
} else if (un_codepoint <= 0x10ffff) { } 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)++ = 0b11110000 | (un_codepoint >> 18 & 0b111);
*(*dst)++ = 0b10000000 | (un_codepoint >> 12 & 0b111111); *(*dst)++ = 0b10000000 | (un_codepoint >> 12 & 0b111111);
*(*dst)++ = 0b10000000 | (un_codepoint >> 6 & 0b111111); *(*dst)++ = 0b10000000 | (un_codepoint >> 6 & 0b111111);
*(*dst)++ = 0b10000000 | (un_codepoint >> 0 & 0b111111); *(*dst)++ = 0b10000000 | (un_codepoint >> 0 & 0b111111);
header->len += 4; header->len += 4;
} else { } else { // Illegal codepoint
return set_jerrno(StringBadUnicode); return set_jerrno(StringBadUnicode);
} }
// We finished parsing the \uXXXX escape
esc_unicode = -1; esc_unicode = -1;
} }
} else if (esc) { } else if (esc) {
@ -112,6 +145,7 @@ static inline int json_parse_string(const char **buf, const char *buf_end, uint8
case '"': case '"':
case '\\': case '\\':
case '/': // For some reason you can escape a slash in JSON case '/': // For some reason you can escape a slash in JSON
// Those stay the same
r = c; r = c;
break; break;
case 'b': case 'b':
@ -136,10 +170,11 @@ static inline int json_parse_string(const char **buf, const char *buf_end, uint8
return set_jerrno(StringBadEscape); return set_jerrno(StringBadEscape);
} }
if (c != 'u') { if (c != 'u') { // \u is the only escape that doesn't immediatly produce a character
if (*dst + 1 >= dst_end) { if (*dst + 1 >= dst_end) {
return set_jerrno(DstOverflow); return set_jerrno(DstOverflow);
} }
// *(*dst)++ => set **dst to RHS and increment *dst
*(*dst)++ = r; *(*dst)++ = r;
header->len++; header->len++;
} }
@ -149,32 +184,44 @@ static inline int json_parse_string(const char **buf, const char *buf_end, uint8
if (c == '\\') { if (c == '\\') {
esc = true; esc = true;
continue; continue;
} else if (c == '"') { } else if (c == '"') { // Closing quote
int padded_len = align_8(header->len); int padded_len = align_8(header->len);
if (*dst + (padded_len - header->len) >= dst_end) // Ensure enough space in dst for padding
if (*dst + (padded_len - header->len) >= dst_end) {
return set_jerrno(DstOverflow); return set_jerrno(DstOverflow);
for (; padded_len > header->len; padded_len--) }
// Pad to 8 align
for (; padded_len > header->len; padded_len--) {
*(*dst)++ = '\0'; *(*dst)++ = '\0';
}
// Skip "
(*buf)++; (*buf)++;
return 0; return 0;
} else if ((c < ' ' && c != '\t') || c == 0x7f) { } 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; jerrno = StringBadChar;
return -1; return -1;
} }
if (*dst + 1 >= dst_end)
if (*dst + 1 >= dst_end) {
return set_jerrno(DstOverflow); return set_jerrno(DstOverflow);
}
// *(*dst)++ => set **dst to RHS and increment *dst
*(*dst)++ = c; *(*dst)++ = c;
header->len++; header->len++;
} }
} }
// The only way to get out of the loop is if *buf >= buf_end // The only way to get out of the loop is if *buf >= buf_end: buffer overflow
return set_jerrno(SrcOverflow); return set_jerrno(SrcOverflow);
} }
// *dst must be 8 aligned // *dst must be 8 aligned
static int json_parse_number(const char **buf, const char *buf_end, uint8_t **restrict dst, static int json_parse_number(const char **buf, const char *buf_end, uint8_t **restrict dst,
const uint8_t *dst_end) { const uint8_t *dst_end) {
// Ensure enough space for header and value
if (*dst + sizeof(JSONHeader) + sizeof(double) >= dst_end) { if (*dst + sizeof(JSONHeader) + sizeof(double) >= dst_end) {
return set_jerrno(DstOverflow); return set_jerrno(DstOverflow);
} }
@ -186,18 +233,24 @@ static int json_parse_number(const char **buf, const char *buf_end, uint8_t **re
header->type = (uint32_t)Number; header->type = (uint32_t)Number;
header->len = sizeof(double); header->len = sizeof(double);
*value = 0.0;
double sign = 1.0; double sign = 1.0;
if (**buf == '-') { if (**buf == '-') {
(*buf)++; (*buf)++; // Skip -
sign = -1.0; sign = -1.0;
} }
if (*buf >= buf_end) // There has to be at least one digit
if (*buf >= buf_end) {
return set_jerrno(SrcOverflow); return set_jerrno(SrcOverflow);
}
if (**buf != '0') { if (**buf != '0') {
// If the first character is not a zero we have a pententially mutli digit number
for (; *buf < buf_end; (*buf)++) { for (; *buf < buf_end; (*buf)++) {
char c = **buf; char c = **buf;
if (c < '0' || c > '9') { if (c < '0' || c > '9') { // if c isn't a number
break; break;
} }
@ -205,54 +258,81 @@ static int json_parse_number(const char **buf, const char *buf_end, uint8_t **re
*value += (double)(c - '0'); *value += (double)(c - '0');
} }
} else { } else {
// If c is zero we can't have anything else (for the integer part)
(*buf)++; (*buf)++;
} }
// If there another character and its a . we have a fractional part
if (*buf < buf_end && **buf == '.') { if (*buf < buf_end && **buf == '.') {
// Decimal place
double place = 0.1; double place = 0.1;
(*buf)++; // Skip dot
if (*buf >= buf_end) (*buf)++; // Skip .
// There must be at least one digit after the dot
if (*buf >= buf_end) {
return set_jerrno(SrcOverflow); return set_jerrno(SrcOverflow);
if (**buf < '0' || **buf > '9') }
if (**buf < '0' || **buf > '9') {
return set_jerrno(NumberBadChar); return set_jerrno(NumberBadChar);
}
for (; *buf < buf_end; (*buf)++) { for (; *buf < buf_end; (*buf)++) {
char c = **buf; char c = **buf;
if (c < '0' || c > '9')
if (c < '0' || c > '9') {
break; break;
}
double digit = (double)(c - '0'); double digit = (double)(c - '0');
*value += digit * place; *value += digit * place;
place *= 0.1; place *= 0.1;
} }
} }
// if theres at least one more character and its an e or an E we got an exponent
if (*buf < buf_end && (**buf == 'e' || **buf == 'E')) { if (*buf < buf_end && (**buf == 'e' || **buf == 'E')) {
double exp = 0.0; double exp = 0.0;
double exp_sign = 1.0; double exp_sign = 1.0;
(*buf)++; // Skip e/E (*buf)++; // Skip e/E
if (*buf >= buf_end)
return set_jerrno(SrcOverflow);
if (**buf == '+' || **buf == '-') { // There must be at least one more character (a digit or a sign followed by digit(s))
exp_sign = **buf == '-' ? -1.0 : 1.0; if (*buf >= buf_end) {
(*buf)++;
if (*buf >= buf_end)
return set_jerrno(SrcOverflow); return set_jerrno(SrcOverflow);
} }
// Handle sign of exponent
if (**buf == '+' || **buf == '-') {
exp_sign = **buf == '-' ? -1.0 : 1.0;
(*buf)++; // Skip sign
// If there's a sign there must be at least one digit following it
if (*buf >= buf_end) {
return set_jerrno(SrcOverflow);
}
}
// Parse exponent
for (; *buf < buf_end; (*buf)++) { for (; *buf < buf_end; (*buf)++) {
char c = **buf; char c = **buf;
if (c < '0' || c > '9')
if (c < '0' || c > '9') {
break; break;
}
exp *= 10; exp *= 10;
exp += (double)(c - '0'); exp += (double)(c - '0');
} }
// Apply exponent
exp *= exp_sign; exp *= exp_sign;
*value *= pow(10.0, exp); *value *= pow(10.0, exp);
} }
// Apply sign
*value *= sign; *value *= sign;
return 0; return 0;
@ -261,8 +341,8 @@ static int json_parse_number(const char **buf, const char *buf_end, uint8_t **re
// *dst must be 8 aligned // *dst must be 8 aligned
static int json_parse_boolean(const char **buf, const char *buf_end, uint8_t **restrict dst, static int json_parse_boolean(const char **buf, const char *buf_end, uint8_t **restrict dst,
const uint8_t *dst_end) { const uint8_t *dst_end) {
// Ensure enough space for header and value
if (*dst + sizeof(JSONHeader) + 8 >= dst_end) { if (*dst + sizeof(JSONHeader) + 8 >= dst_end) { // 8: sizeof(uint64_t)
return set_jerrno(DstOverflow); return set_jerrno(DstOverflow);
} }
@ -273,32 +353,35 @@ static int json_parse_boolean(const char **buf, const char *buf_end, uint8_t **r
header->type = (uint32_t)Boolean; header->type = (uint32_t)Boolean;
header->len = 8; header->len = 8;
if (**buf == 't') { if (**buf == 't') { // The value can only be true, so we check it against that
if (*buf + 4 > buf_end) if (*buf + 4 > buf_end)
return set_jerrno(SrcOverflow); return set_jerrno(SrcOverflow);
if (strncmp(*buf, "true", 4) != 0) { if (strncmp(*buf, "true", 4) != 0) {
return set_jerrno(BadKeyword); return set_jerrno(BadKeyword);
} }
*buf += 4; *buf += 4;
*value = 1; *value = 1;
} else if (**buf == 'f') { } else if (**buf == 'f') { // The value can only be false
if (*buf + 5 > buf_end) if (*buf + 5 > buf_end)
return set_jerrno(SrcOverflow); return set_jerrno(SrcOverflow);
if (strncmp(*buf, "false", 5) != 0) { if (strncmp(*buf, "false", 5) != 0) {
return set_jerrno(BadKeyword); return set_jerrno(BadKeyword);
} }
*buf += 5; *buf += 5;
*value = 0; *value = 0;
} else { } else {
return set_jerrno(BadKeyword); return set_jerrno(BadKeyword);
} }
return 0; return 0;
} }
// *dst must be 8 aligned // *dst must be 8 aligned
static int json_parse_null(const char **buf, const char *buf_end, uint8_t **restrict dst, static int json_parse_null(const char **buf, const char *buf_end, uint8_t **restrict dst,
const uint8_t *dst_end) { const uint8_t *dst_end) {
// Ensure enough size for the header (no value)
if (*dst + sizeof(JSONHeader) >= dst_end) { if (*dst + sizeof(JSONHeader) >= dst_end) {
return set_jerrno(DstOverflow); return set_jerrno(DstOverflow);
} }
@ -309,121 +392,155 @@ static int json_parse_null(const char **buf, const char *buf_end, uint8_t **rest
header->type = (uint32_t)Null; header->type = (uint32_t)Null;
header->len = 0; header->len = 0;
// Check that the word is indeed null
if (*buf + 4 > buf_end) if (*buf + 4 > buf_end)
return set_jerrno(SrcOverflow); return set_jerrno(SrcOverflow);
if (strncmp(*buf, "null", 4) != 0) { if (strncmp(*buf, "null", 4) != 0) {
return set_jerrno(BadKeyword); return set_jerrno(BadKeyword);
} }
*buf += 4; *buf += 4;
return 0; return 0;
} }
// *dst must be 8 aligned // *dst must be 8 aligned
static int json_parse_array(const char **buf, const char *buf_end, uint8_t **restrict dst, static int json_parse_array(const char **buf, const char *buf_end, uint8_t **restrict dst,
const uint8_t *dst_end) { const uint8_t *dst_end) {
// Ensure enough space for the header
if (*dst + sizeof(JSONHeader) >= dst_end) { if (*dst + sizeof(JSONHeader) >= dst_end) {
return set_jerrno(DstOverflow); return set_jerrno(DstOverflow);
} }
// Setup header
JSONHeader *header = (JSONHeader *)(*dst); JSONHeader *header = (JSONHeader *)(*dst);
*dst += sizeof(JSONHeader); *dst += sizeof(JSONHeader);
// Keep track of pointer to the start of "value" of the array, used to compute the final length
uint8_t *dst_arr_start = *dst; uint8_t *dst_arr_start = *dst;
header->type = (uint32_t)Array; header->type = (uint32_t)Array;
(*buf)++; // Skip [ (*buf)++; // Skip [
// skip initial whitespace // skip initial whitespace
for (; *buf < buf_end && is_whitespace(**buf); (*buf)++) skip_whitespaces(buf, buf_end);
;
if (*buf == buf_end) // There should be at least one more character (a value or ])
if (*buf == buf_end) {
return set_jerrno(SrcOverflow); return set_jerrno(SrcOverflow);
if (**buf == ']') { }
if (**buf == ']') { // Array is empty
header->len = 0; header->len = 0;
return 0; return 0;
} }
while (1) { while (1) {
if (json_parse_value(buf, buf_end, dst, dst_end) != 0) // Try to parse a value
if (json_parse_value(buf, buf_end, dst, dst_end) != 0) {
return -1; return -1;
for (; *buf < buf_end && is_whitespace(**buf); (*buf)++) }
; // Skip whitespaces after value
if (*buf == buf_end) skip_whitespaces(buf, buf_end);
// There should be at least one more char (, or ])
if (*buf == buf_end) {
return set_jerrno(SrcOverflow); return set_jerrno(SrcOverflow);
}
if (**buf == ',') { if (**buf == ',') {
// Skip , and go for another iteration
(*buf)++; (*buf)++;
} else if (**buf == ']') { } else if (**buf == ']') {
// Skip ] and finish
(*buf)++; (*buf)++;
break; break;
} else { } else {
return set_jerrno(BadChar); return set_jerrno(BadChar);
} }
} }
// Compute len
header->len = *dst - dst_arr_start; header->len = *dst - dst_arr_start;
return 0; return 0;
} }
// *dst must be 8 aligned // *dst must be 8 aligned
static int json_parse_object(const char **buf, const char *buf_end, uint8_t **restrict dst, static int json_parse_object(const char **buf, const char *buf_end, uint8_t **restrict dst,
const uint8_t *dst_end) { const uint8_t *dst_end) {
// Esnure enough space for the header
if (*dst + sizeof(JSONHeader) >= dst_end) { if (*dst + sizeof(JSONHeader) >= dst_end) {
return set_jerrno(DstOverflow); return set_jerrno(DstOverflow);
} }
// Setup header
JSONHeader *header = (JSONHeader *)(*dst); JSONHeader *header = (JSONHeader *)(*dst);
*dst += sizeof(JSONHeader); *dst += sizeof(JSONHeader);
// Keep track of pointer to start of value to compute length later
uint8_t *dst_obj_start = *dst; uint8_t *dst_obj_start = *dst;
header->type = (uint32_t)Object; header->type = (uint32_t)Object;
(*buf)++; // Skip { (*buf)++; // Skip {
for (; *buf < buf_end && is_whitespace(**buf); (*buf)++) // Skip initial whitespace (after '{')
; skip_whitespaces(buf, buf_end);
if (*buf == buf_end) // There should be at least one more char (a value or })
if (*buf == buf_end) {
return set_jerrno(SrcOverflow); return set_jerrno(SrcOverflow);
}
if (**buf == '}') { if (**buf == '}') {
// The object is empty
header->len = 0; header->len = 0;
return 0; return 0;
} }
while (1) { while (1) {
// Skip whitespace before key // Skip whitespace before key
for (; *buf < buf_end && is_whitespace(**buf); (*buf)++) skip_whitespaces(buf, buf_end);
; // Try to parse key
// Parse key if (json_parse_string(buf, buf_end, dst, dst_end) != 0) {
if (json_parse_string(buf, buf_end, dst, dst_end) != 0)
return -1; return -1;
}
// Skip whitespace after key // Skip whitespace after key
for (; *buf < buf_end && is_whitespace(**buf); (*buf)++) skip_whitespaces(buf, buf_end);
;
// There should be at least one char
if (*buf == buf_end)
return set_jerrno(SrcOverflow);
// There should be a colon // There should be a colon
if (**buf != ':') if (*buf == buf_end) {
return set_jerrno(ObjectBadChar);
// Skip colon
(*buf)++;
// Parse value (takes char of whitespace)
if (json_parse_value(buf, buf_end, dst, dst_end) != 0)
return -1;
// Skip whitespace after value
for (; *buf < buf_end && is_whitespace(**buf); (*buf)++)
;
// There should be at least one char (} or ,)
if (*buf == buf_end)
return set_jerrno(SrcOverflow); return set_jerrno(SrcOverflow);
}
if (**buf != ':') {
return set_jerrno(ObjectBadChar);
}
(*buf)++;
// Try to parse value (takes care of whitespaces)
if (json_parse_value(buf, buf_end, dst, dst_end) != 0) {
return -1;
}
// Skip whitespace after value
skip_whitespaces(buf, buf_end);
// There should be at least one char (} or ,)
if (*buf == buf_end) {
return set_jerrno(SrcOverflow);
}
if (**buf == ',') { if (**buf == ',') {
// Skip , and go for another iteration
(*buf)++; (*buf)++;
} else if (**buf == '}') { } else if (**buf == '}') {
// Skip } and finish
(*buf)++; (*buf)++;
break; break;
} else { } else {
return set_jerrno(BadChar); return set_jerrno(BadChar);
} }
} }
//
// Compute length
header->len = *dst - dst_obj_start; header->len = *dst - dst_obj_start;
return 0; return 0;
} }
@ -431,6 +548,7 @@ static int json_parse_object(const char **buf, const char *buf_end, uint8_t **re
static int json_parse_value(const char **buf, const char *buf_end, uint8_t **restrict dst, static int json_parse_value(const char **buf, const char *buf_end, uint8_t **restrict dst,
const uint8_t *dst_end) { const uint8_t *dst_end) {
for (; *buf < buf_end; (*buf)++) { for (; *buf < buf_end; (*buf)++) {
// Ignore initial whitespaces
if (is_whitespace(**buf)) if (is_whitespace(**buf))
continue; continue;
@ -462,26 +580,32 @@ static int json_parse_value(const char **buf, const char *buf_end, uint8_t **res
return set_jerrno(BadChar); return set_jerrno(BadChar);
} }
} }
if (*buf == buf_end)
if (*buf == buf_end) {
return set_jerrno(SrcOverflow); return set_jerrno(SrcOverflow);
}
return 0; return 0;
} }
int json_parse(const char *src, size_t src_len, uint8_t *dst, size_t dst_len) { int json_parse(const char *src, size_t src_len, uint8_t *dst, size_t dst_len) {
memset(dst, 0, dst_len);
const char *buf = src; const char *buf = src;
const char *buf_end = src + src_len; const char *buf_end = src + src_len;
uint8_t *dst_end = dst + dst_len; uint8_t *dst_end = dst + dst_len;
int rc = json_parse_value(&buf, buf_end, &dst, dst_end); int rc = json_parse_value(&buf, buf_end, &dst, dst_end);
// Set location to the difference between were we got to and where we started
jerr_index = buf - src; jerr_index = buf - src;
return rc; return rc;
} }
void json_print_value(uint8_t **buf) { void json_print_value_priv(uint8_t **buf) {
JSONHeader *header = (JSONHeader *)*buf; JSONHeader *header = (JSONHeader *)*buf;
*buf += sizeof(header); *buf += sizeof(header);
switch (header->type) { switch (header->type) {
case String: case String:
// TODO: escapes
printf("\"%.*s\"", header->len, *buf); printf("\"%.*s\"", header->len, *buf);
*buf += align_8(header->len); *buf += align_8(header->len);
break; break;
@ -507,7 +631,7 @@ void json_print_value(uint8_t **buf) {
uint8_t *end = *buf + header->len; uint8_t *end = *buf + header->len;
printf("["); printf("[");
while (1) { while (1) {
json_print_value(buf); json_print_value_priv(buf);
if (*buf < end) { if (*buf < end) {
printf(","); printf(",");
} else { } else {
@ -520,9 +644,9 @@ void json_print_value(uint8_t **buf) {
uint8_t *end = *buf + header->len; uint8_t *end = *buf + header->len;
printf("{"); printf("{");
while (1) { while (1) {
json_print_value(buf); json_print_value_priv(buf);
printf(":"); printf(":");
json_print_value(buf); json_print_value_priv(buf);
if (*buf < end) { if (*buf < end) {
printf(","); printf(",");
} else { } else {
@ -534,21 +658,18 @@ void json_print_value(uint8_t **buf) {
} }
} }
struct Test { // /!\ doesn't handle strings well
double a; void json_print_value(uint8_t *buf) { json_print_value_priv(&buf); }
char *b;
};
const JSONAdapter TestAdapter[] = {
{".a", Number, offsetof(struct Test, a)},
{".b", String, offsetof(struct Test, b)},
};
// Loop over adapters and set accordingly
static void json_adapt_set(uint8_t *buf, JSONAdapter *adapters, size_t adapter_count, void *ptr, char *path) { static void json_adapt_set(uint8_t *buf, JSONAdapter *adapters, size_t adapter_count, void *ptr, char *path) {
JSONHeader *header = (JSONHeader *)buf; JSONHeader *header = (JSONHeader *)buf;
for (int i = 0; i < adapter_count; i++) { for (int i = 0; i < adapter_count; i++) {
if (strcmp(path, adapters[i].path) == 0 && header->type == adapters[i].type) { if (strcmp(path, adapters[i].path) == 0 && header->type == adapters[i].type) {
void *p = ptr + adapters[i].offset; void *p = ptr + adapters[i].offset;
switch (header->type) { switch (header->type) {
case String: { case String: {
char *v = malloc(header->len + 1); char *v = malloc(header->len + 1);
@ -567,6 +688,7 @@ static void json_adapt_set(uint8_t *buf, JSONAdapter *adapters, size_t adapter_c
} }
} }
// Run adapters on a value
static void json_adapt_priv(uint8_t **buf, JSONAdapter *adapters, size_t adapter_count, void *ptr, static void json_adapt_priv(uint8_t **buf, JSONAdapter *adapters, size_t adapter_count, void *ptr,
char *full_path, char *path) { char *full_path, char *path) {
JSONHeader *header = (JSONHeader *)*buf; JSONHeader *header = (JSONHeader *)*buf;
@ -611,6 +733,7 @@ static void json_adapt_priv(uint8_t **buf, JSONAdapter *adapters, size_t adapter
} }
} }
// Run adapters on a json value
void json_adapt(uint8_t *buf, JSONAdapter *adapters, size_t adapter_count, void *ptr) { void json_adapt(uint8_t *buf, JSONAdapter *adapters, size_t adapter_count, void *ptr) {
char path[512] = "."; char path[512] = ".";
json_adapt_priv(&buf, adapters, adapter_count, ptr, path, path); json_adapt_priv(&buf, adapters, adapter_count, ptr, path, path);

5
json.h
View File

@ -53,6 +53,7 @@ static const char *JSONErrorMessage[JERRORNO_MAX + 1] = {
}; };
#endif #endif
// See client.c for usage of adapters
typedef struct { typedef struct {
char *path; char *path;
JSONType type; JSONType type;
@ -61,7 +62,9 @@ typedef struct {
void json_adapt(uint8_t *buf, JSONAdapter *adapters, size_t adapter_count, void *ptr); 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); 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(); const char *json_strerr();
size_t json_err_loc(); size_t json_errloc();
JSONError json_errno();
#endif #endif

13
main.c
View File

@ -1,5 +1,3 @@
#include "main.h"
#include "client.h" #include "client.h"
#include "hid.h" #include "hid.h"
#include "server.h" #include "server.h"
@ -8,7 +6,6 @@
#include <pthread.h> #include <pthread.h>
#include <stdint.h> #include <stdint.h>
#include <stdio.h> #include <stdio.h>
#include <stdlib.h>
#include <string.h> #include <string.h>
const char *USAGE[] = { const char *USAGE[] = {
@ -16,18 +13,12 @@ const char *USAGE[] = {
"jsfw server [port]\n", "jsfw server [port]\n",
}; };
uint16_t parse_port(const char *str) {
long long n = atoll(str);
if (n <= 0 || n > UINT16_MAX)
panicf("Invalid port: Expected a number in the range 1..%d, got '%s'\n", UINT16_MAX, str);
return n;
}
void server(uint16_t port) { void server(uint16_t port) {
printf("[Server (port: %u)]\n\n", port); printf("[Server (0.0.0.0:%u)]\n\n", port);
pthread_t thread; pthread_t thread;
pthread_create(&thread, NULL, hid_thread, NULL); pthread_create(&thread, NULL, hid_thread, NULL);
server_run(port); server_run(port);
} }

5
main.h
View File

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

9
net.c
View File

@ -2,15 +2,6 @@
#include <stdio.h> #include <stdio.h>
Message msg_device_info() {
MessageDeviceInfo m;
m.code = DeviceInfo;
Message s;
s.device_info = m;
return s;
}
// 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
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) {

28
server.c
View File

@ -1,3 +1,4 @@
#include "const.h"
#include "hid.h" #include "hid.h"
#include "net.h" #include "net.h"
#include "util.h" #include "util.h"
@ -8,7 +9,6 @@
#include <netinet/tcp.h> #include <netinet/tcp.h>
#include <poll.h> #include <poll.h>
#include <pthread.h> #include <pthread.h>
#include <stdbool.h>
#include <stdint.h> #include <stdint.h>
#include <stdio.h> #include <stdio.h>
#include <stdlib.h> #include <stdlib.h>
@ -16,6 +16,7 @@
#include <sys/socket.h> #include <sys/socket.h>
#include <unistd.h> #include <unistd.h>
// Arguments for a connecion thread
struct Connection { struct Connection {
int socket; int socket;
uint32_t id; uint32_t id;
@ -26,21 +27,17 @@ void *server_handle_conn(void *args_) {
printf("CONN(%u): start\n", args->id); printf("CONN(%u): start\n", args->id);
int enable = true; if (setsockopt(args->socket, SOL_SOCKET, SO_KEEPALIVE, &TCP_KEEPALIVE_ENABLE, sizeof(int)) != 0)
int idle_time = 10;
int keep_count = 5;
int keep_interval = 2;
if (setsockopt(args->socket, SOL_SOCKET, SO_KEEPALIVE, &enable, sizeof(enable)) != 0)
printf("ERR(server_handle_conn): Enabling socket keepalives on client\n"); printf("ERR(server_handle_conn): Enabling socket keepalives on client\n");
if (setsockopt(args->socket, SOL_TCP, TCP_KEEPIDLE, &idle_time, sizeof(idle_time)) != 0) if (setsockopt(args->socket, SOL_TCP, TCP_KEEPIDLE, &TCP_KEEPALIVE_IDLE_TIME, sizeof(int)) != 0)
printf("ERR(server_handle_conn): Setting initial ERR()-time value\n"); printf("ERR(server_handle_conn): Setting initial idle-time value\n");
if (setsockopt(args->socket, SOL_TCP, TCP_KEEPCNT, &keep_count, sizeof(keep_count)) != 0) if (setsockopt(args->socket, SOL_TCP, TCP_KEEPCNT, &TCP_KEEPALIVE_RETRY_COUNT, sizeof(int)) != 0)
printf("ERR(server_handle_conn): Setting idle retry count\n"); printf("ERR(server_handle_conn): Setting idle retry count\n");
if (setsockopt(args->socket, SOL_TCP, TCP_KEEPINTVL, &keep_interval, sizeof(keep_interval)) != 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))) = {};
PhysicalDevice dev = get_device(); PhysicalDevice dev = get_device();
printf("CONN(%u): got device '%s'\n", args->id, dev.name); printf("CONN(%u): got device '%s'\n", args->id, dev.name);
@ -182,19 +179,22 @@ void server_run(uint16_t port) {
printf("SERVER: start\n"); printf("SERVER: start\n");
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 = {}; struct sockaddr_in addr = {};
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);
if (bind(sock, (struct sockaddr *)&addr, sizeof(addr)) != 0) if (bind(sock, (struct sockaddr *)&addr, sizeof(addr)) != 0) {
panicf("Couldn't bind to the socket\n"); panicf("Couldn't bind to the socket\n");
}
if (listen(sock, 16) != 0) if (listen(sock, 16) != 0) {
panicf("Couldn't listen on socket\n"); panicf("Couldn't listen on socket\n");
}
uint32_t ids = 0; uint32_t ids = 0;
while (1) { while (1) {

19
util.c
View File

@ -1,6 +1,7 @@
#include "util.h" #include "util.h"
#include <stdarg.h> #include <stdarg.h>
#include <stdint.h>
#include <stdio.h> #include <stdio.h>
#include <stdlib.h> #include <stdlib.h>
@ -15,3 +16,21 @@ void panicf(const char *fmt, ...) {
va_end(args); va_end(args);
exit(1); exit(1);
} }
uint16_t parse_port(const char *str) {
long long n = atoll(str);
if (n <= 0 || n > UINT16_MAX)
panicf("Invalid port: Expected a number in the range 1..%d, got '%s'\n", UINT16_MAX, str);
return n;
}
uint8_t parse_hex_digit(char h) {
if (h >= '0' && h <= '9')
return h - '0';
else if (h >= 'a' && h <= 'f')
return h - 'a' + 10;
else if (h >= 'A' && h <= 'F')
return h - 'A' + 10;
else
return 0;
}

11
util.h
View File

@ -1,7 +1,18 @@
// vi:ft=c // vi:ft=c
#ifndef UTIL_H_ #ifndef UTIL_H_
#define UTIL_H_ #define UTIL_H_
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
// Print a formatted message and exit with code 1
void panicf(const char *fmt, ...); void panicf(const char *fmt, ...);
uint16_t parse_port(const char *str);
// Test if the bit with index i is set in the byte array bits
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; }
uint8_t parse_hex_digit(char h);
#endif #endif

23
vec.c
View File

@ -23,12 +23,16 @@ Vec vec_cap(size_t data_size, size_t initial_capacity) {
} }
static inline void vec_grow(Vec *v, size_t cap) { static inline void vec_grow(Vec *v, size_t cap) {
if (v->cap >= cap) if (v->cap >= cap) {
return; return;
}
size_t new_cap = cap > v->cap * 2 ? cap : v->cap * 2; size_t new_cap = cap > v->cap * 2 ? cap : v->cap * 2;
void *new_data = realloc(v->data, new_cap * v->stride); void *new_data = realloc(v->data, new_cap * v->stride);
if (new_data == NULL) if (new_data == NULL) {
handle_alloc_error(); handle_alloc_error();
}
v->data = new_data; v->data = new_data;
v->cap = new_cap; v->cap = new_cap;
} }
@ -43,14 +47,16 @@ void vec_pop(Vec *v, void *data) {
printf("ERR(vec_pop): Trying to pop an element from an empty vector\n"); printf("ERR(vec_pop): Trying to pop an element from an empty vector\n");
return; return;
} }
if (data != NULL) if (data != NULL) {
memcpy(data, v->data + v->stride * (v->len - 1), v->stride); memcpy(data, v->data + v->stride * (v->len - 1), v->stride);
}
v->len--; v->len--;
} }
void *vec_get(Vec *v, size_t index) { void *vec_get(Vec *v, size_t index) {
if (index >= v->len) if (index >= v->len) {
return NULL; return NULL;
}
return v->data + index * v->stride; return v->data + index * v->stride;
} }
@ -80,17 +86,20 @@ void vec_remove(Vec *v, size_t index, void *data) {
} }
void *slot = v->data + index * v->stride; void *slot = v->data + index * v->stride;
if (data != NULL) if (data != NULL) {
memcpy(data, slot, v->stride); memcpy(data, slot, v->stride);
if (index < --v->len) }
if (index < --v->len) {
memmove(slot, slot + v->stride, (v->len - index) * v->stride); memmove(slot, slot + v->stride, (v->len - index) * v->stride);
} }
}
void vec_clear(Vec *v) { v->len = 0; } void vec_clear(Vec *v) { v->len = 0; }
void vec_extend(Vec *v, void *data, size_t len) { void vec_extend(Vec *v, void *data, size_t len) {
if (len == 0) if (len == 0) {
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(v->data + v->stride * v->len, data, v->stride * len);
v->len += len; v->len += len;