diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..80d3293 --- /dev/null +++ b/.clang-format @@ -0,0 +1,2 @@ +BasedOnStyle: LLVM +IndentWidth: 4 diff --git a/.gitignore b/.gitignore index 14f1512..1716794 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,4 @@ .ccls-cache -build +objects jsfw plan diff --git a/Makefile b/Makefile index acff31d..90aa386 100644 --- a/Makefile +++ b/Makefile @@ -1,13 +1,13 @@ Q=@ CC=gcc -CFLAGS=-g -Wall +CFLAGS=-g -Wall -Wno-format-truncation LDFLAGS= -BUILD_DIR=./build +BUILD_DIR=./objects BIN=jsfw -RUNARGS=client /dev/input/js0 localhost 7776 +RUNARGS=client localhost 7776 -SOURCES=main.c hid.c +SOURCES=$(wildcard *.c) OBJECTS:=$(patsubst %.c,$(BUILD_DIR)/%.o,$(SOURCES)) diff --git a/client.h b/client.h new file mode 100644 index 0000000..80e029f --- /dev/null +++ b/client.h @@ -0,0 +1,6 @@ +#ifndef CLIENT_H +#define CLIENT_H + + + +#endif diff --git a/hashmap.c b/hashmap.c index 942cbce..06d5567 100644 --- a/hashmap.c +++ b/hashmap.c @@ -1,10 +1,15 @@ -#include "hashmap.h" +#include +#include #include -int seed = 0; +#include "hashmap.h" +#include "util.h" + +uint32_t seed = 0; void init_seed() { - if(seed) return; + if (seed) + return; seed = random(); } @@ -12,28 +17,16 @@ void init_seed() { // Is taken from the internet because I needed a simple hash function // -------------------------------------------------------- -#define PRIME1 0x9E3779B1U /*!< 0b10011110001101110111100110110001 */ -#define PRIME2 0x85EBCA77U /*!< 0b10000101111010111100101001110111 */ -#define PRIME3 0xC2B2AE3DU /*!< 0b11000010101100101010111000111101 */ -#define PRIME4 0x27D4EB2FU /*!< 0b00100111110101001110101100101111 */ -#define PRIME5 0x165667B1U /*!< 0b00010110010101100110011110110001 */ +#define PRIME1 0x9E3779B1U /*!< 0b10011110001101110111100110110001 */ +#define PRIME2 0x85EBCA77U /*!< 0b10000101111010111100101001110111 */ +#define PRIME3 0xC2B2AE3DU /*!< 0b11000010101100101010111000111101 */ +#define PRIME4 0x27D4EB2FU /*!< 0b00100111110101001110101100101111 */ +#define PRIME5 0x165667B1U /*!< 0b00010110010101100110011110110001 */ -uint32_t _rotl(const uint32_t value, int shift) { - if ((shift &= sizeof(value)*8 - 1) == 0) - return value; - return (value << shift) | (value >> (sizeof(value)*8 - shift)); -} - -uint32_t _rotr(const uint32_t value, int shift) { - if ((shift &= sizeof(value)*8 - 1) == 0) - return value; - return (value >> shift) | (value << (sizeof(value)*8 - shift)); -} - -uint32_t hash(uint8_t * data, int len) { - int end = len; - int offset = 0; - int h32; +uint32_t xxhash32(uint8_t *data, size_t len) { + size_t end = len; + size_t offset = 0; + uint32_t h32; if (len >= 16) { int limit = end - 16; uint32_t v1 = seed + PRIME1 + PRIME2; @@ -42,38 +35,40 @@ uint32_t hash(uint8_t * data, int len) { uint32_t v4 = seed - PRIME1; do { - v1 += (*(uint32_t*)(data + offset)) * PRIME2; - v1 = _rotl(v1, 13); + v1 += (*(uint32_t *)(data + offset)) * PRIME2; + v1 = rotl(v1, 13); v1 *= PRIME1; offset += 4; - v2 += (*(uint32_t*)(data + offset)) * PRIME2; - v2 = _rotl(v2, 13); + v2 += (*(uint32_t *)(data + offset)) * PRIME2; + v2 = rotl(v2, 13); v2 *= PRIME1; offset += 4; - v3 += (*(uint32_t*)(data + offset)) * PRIME2; - v3 = _rotl(v3, 13); + v3 += (*(uint32_t *)(data + offset)) * PRIME2; + v3 = rotl(v3, 13); v3 *= PRIME1; offset += 4; - v4 += (*(uint32_t*)(data + offset)) * PRIME2; - v4 = _rotl(v4, 13); + v4 += (*(uint32_t *)(data + offset)) * PRIME2; + v4 = rotl(v4, 13); v4 *= PRIME1; offset += 4; - } while(offset <= limit); + } while (offset <= limit); // main loop ends // mix - h32 = _rotl(v1, 1) + _rotl(v2, 7) + _rotl(v3, 12) + _rotl(v4, 18); + h32 = rotl(v1, 1) + rotl(v2, 7) + rotl(v3, 12) + rotl(v4, 18); } else { h32 = seed + PRIME5; } - for(h32 += len; offset <= end - 4; offset += 4) { - h32 += (*(uint32_t*)(data + offset)) * PRIME3; - h32 = _rotl(h32, 17) * PRIME4; + if (end > 4) { + for (h32 += len; offset <= end - 4; offset += 4) { + h32 += (*(uint32_t *)(data + offset)) * PRIME3; + h32 = rotl(h32, 17) * PRIME4; + } } - while(offset < end) { + while (offset < end) { h32 += (data[offset] & 255) * PRIME5; - h32 = _rotl(h32, 11) * PRIME1; + h32 = rotl(h32, 11) * PRIME1; ++offset; } @@ -88,4 +83,264 @@ uint32_t hash(uint8_t * data, int len) { // -------------------------------------------------------- // To here +// Po2 +#define INIT_BUCKET_COUNT 32 +#define HEADER_SIZE sizeof(void *) +#define GROW_THRESHOLD 75 +// The size of an entry for a data size of +#define ENTRY(size) (HEADER_SIZE + size) +// Each entry is composed of a 8 bytes header and the data (or garbage if there +// is none). The 8 bytes header is either a non null pointer to the key or zero +// if the bucket is empty +static inline void handle_alloc_error() { + printf("Error when allocating (OOM?)\n"); + exit(1); +} + +Map map_new(size_t data_size) { + Map map = {}; + map.bucket_count = INIT_BUCKET_COUNT; + map.data_size = data_size; + map.used = 0; + map.buckets = calloc(INIT_BUCKET_COUNT, ENTRY(data_size)); + map.mask = INIT_BUCKET_COUNT - 1; + if (!map.buckets) + handle_alloc_error(); + return map; +} + +// Double the size of the map +void map_grow(Map *map) { + size_t entry_size = ENTRY(map->data_size); + + size_t new_bucket_count = map->bucket_count * 2; + void *new_alloc = calloc(new_bucket_count, entry_size); + size_t new_mask = (map->mask << 1) + 1; + if (!new_alloc) + handle_alloc_error(); + void *current = map->buckets; + + for (int i = 0; i < map->bucket_count; i++) { + void *header = *(void **)current; + if (header == NULL) + continue; // skip if unused + size_t len = *(size_t *)header; + void *key = header + sizeof(size_t); + + uint32_t hash = xxhash32(key, len); + uint32_t index = hash & new_mask; + void *dst_bucket = new_alloc + index * entry_size; + + while (*(void **)dst_bucket != NULL) { + dst_bucket += entry_size; + index++; + if (index == new_bucket_count) { + index = 0; + dst_bucket = new_alloc; + } + } + + *(void **)dst_bucket = header; // set key of new bucket + memcpy(dst_bucket + HEADER_SIZE, current + HEADER_SIZE, + map->data_size); // set value of new bucket + + current += entry_size; + } + + free(map->buckets); + map->buckets = new_alloc; + map->mask = new_mask; + map->bucket_count = new_bucket_count; +} + +void map_insert(Map *map, uint8_t *key, size_t key_len, void *data) { + uint32_t hash = xxhash32(key, key_len); + size_t index = hash & map->mask; + size_t entry_size = ENTRY(map->data_size); + void *bucket = map->buckets + index * entry_size; + // Go to next empty bucket + while (*(void **)bucket != NULL) { + bucket += entry_size; + index++; + if (index == map->bucket_count) { + index = 0; + bucket = map->buckets; + } + } + // memory for the key: the size + the data + void *key_buf = malloc(sizeof(size_t) + key_len); + if (!key_buf) + handle_alloc_error(); + *(size_t *)key_buf = key_len; // write key_len + memcpy(key_buf + sizeof(size_t), key, key_len); // write key + + *(void **)bucket = key_buf; + memcpy(bucket + HEADER_SIZE, data, map->data_size); + map->used++; + + if (map->used * 100 / map->bucket_count > GROW_THRESHOLD) { + map_grow(map); + } +} + +inline static void *map_find(Map *map, uint8_t *key, size_t key_len) { + uint32_t hash = xxhash32(key, key_len); + size_t index = hash & map->mask; + size_t entry_size = ENTRY(map->data_size); + void *bucket = map->buckets + index * entry_size; + size_t start_index = index; + while (1) { + void *header = *(void **)bucket; + if (header) { + size_t cur_key_len = *(size_t *)header; + void *cur_key = header + sizeof(size_t); + if (cur_key_len == key_len && memcmp(cur_key, key, key_len) == 0) + break; // We found the bucket (usally the first one) + } + + bucket += entry_size; + index++; + // Go back to begining if we went to the end; + if (index == map->bucket_count) { + index = 0; + bucket = map->buckets; + } + // If we went over every entry without finding the bucket + if (index == start_index) { + return NULL; + } + } + return bucket; +} + +bool map_contains(Map *map, uint8_t *key, size_t key_len) { + return map_find(map, key, key_len) != NULL; +} + +void map_get(Map *map, uint8_t *key, size_t key_len, void *data) { + void *bucket = map_find(map, key, key_len); + if (!bucket) { + printf("ERR (map_get): key not in map\n"); + return; + } + memcpy(data, bucket + HEADER_SIZE, map->data_size); +} + +void map_remove(Map *map, uint8_t *key, size_t key_len, void *data) { + void *bucket = map_find(map, key, key_len); + if (!bucket) { + printf("ERR (map_remove): key not in map\n"); + return; + } + + if (data != NULL) + memcpy(data, bucket + HEADER_SIZE, map->data_size); + + void *key_ptr = *(void **)bucket; + free(key_ptr); + *(void **)bucket = NULL; + map->used--; +} + +static inline size_t map_next_index_from(Map *map, size_t from) { + if (from >= map->bucket_count) + return -1; + for (int i = from; i < map->bucket_count; i++) { + void *bucket = map->buckets + i * ENTRY(map->data_size); + if (*(void **)bucket != NULL) + return i; + } + return -1; +} + +MapIter map_iter(Map *map) { + MapIter iter; + iter.map = map; + iter.next = map_next_index_from(map, 0); + return iter; +} + +bool map_iter_has_next(MapIter *iter) { return iter->next != -1; } + +void map_iter_next(MapIter *iter, void **key, size_t *key_len, void **data) { + void *bucket = + iter->map->buckets + ENTRY(iter->map->data_size) * iter->next; + if (key) + *key = (*(void **)bucket) + sizeof(size_t); + if (key_len) + *key_len = **(size_t **)bucket; + if (data) + *data = bucket + HEADER_SIZE; + iter->next = map_next_index_from(iter->map, iter->next + 1); +} + +void _center(size_t length, char *format, uintmax_t value) { + size_t l = snprintf(NULL, 0, format, value); + char str[l + 1]; + int padleft = (length - l) / 2; + int padright = length - l - padleft; + snprintf(str, l + 1, format, value); + printf("%*s%s%*s", padleft, "", str, padright, ""); +} + +void _centers(size_t length, char * s) { + size_t l = strlen(s); + char str[l + 1]; + int padleft = (length - l) / 2; + int padright = length - l - padleft; + snprintf(str, l + 1, "%s", s); + printf("%*s%s%*s", padleft, "", str, padright, ""); +} + +void map_debug(Map map, char *format) { + printf("%s", "┌────────┬───────────────┬───────────┬─────────┐\n"); + printf("%s", "│ map │ buckets count │ data size │ members │\n"); + printf("%s", "├────────┼───────────────┼───────────┼─────────┤\n"); + printf("%s", "│ values │"); + _center(15, "%lu", map.bucket_count); + printf("%s", "│"); + _center(11, "%lu", map.data_size); + printf("%s", "│"); + _center(9, "%lu", map.used); + printf("%s", "│\n"); + printf("%s", "└────────┴───────────────┴───────────┴─────────┘\n"); + bool last_occ = true; + bool last_n = false; + for (size_t i = 0; i < map.bucket_count; i++) { + void * e = map.buckets + map.data_size * i; + bool locc = last_occ; + last_occ = *(void**)e != NULL; + if (!locc && !last_occ) + continue; + if (locc && !last_occ) { + if (i == 0) { + printf("%s", "┌──────────────────────────────────────────┐\n"); + } else if (!last_n) { + printf("%s", + "\033[1A├────┴──────────────────┴──────────────────┤\n"); + } else { + printf("%s", + "\033[1A┌────┴──────────────────┴──────────────────┤\n"); + } + printf("%s", "│ ..... │\n"); + printf("%s", "├────┬──────────────────┬──────────────────┤\n"); + continue; + } + printf("%s", "│"); + _center(4, "%lu", i); + printf("%s", "│"); + void * ptr = *(void**)e; + _centers(18, ptr == NULL ? "N/A" : ptr + sizeof(size_t)); + printf("%s", "│"); + _center(18, format, *(uint32_t *)(e + HEADER_SIZE)); + printf("%s", "│\n"); + printf("├────┼──────────────────┼──────────────────┤\n"); + } + if (last_occ) { + printf("\033[1A└────┴──────────────────┴──────────────────┘\n"); + } else { + printf("\033[1A└──────────────────────────────────────────┘\n"); + } + printf("\n\n"); +} diff --git a/hashmap.h b/hashmap.h index 1db6307..cdcc489 100644 --- a/hashmap.h +++ b/hashmap.h @@ -1,6 +1,68 @@ #ifndef HASHMAP_H #define HASHMAP_H #include +#include +#include +#include +// A hashmap +typedef struct { + // Pointer to allocation for buckets + void * buckets; + // Typically sizeof(T) for Map<_, T> + size_t data_size; + // Always a power of 2 + size_t bucket_count; + // How many used buckets are there + size_t used; + size_t mask; +} Map; +typedef struct { + Map * map; + size_t next; +} MapIter; + +// Create a new map of a type +#define map_of(type) map_new(sizeof(type)) +// Create a new map holding value of a size +Map map_new(size_t data_size); +// Insert a key value pair in the map +void map_insert(Map * map, uint8_t * key, size_t key_len, void * data); +// Test if a key exist within a map +bool map_contains(Map * map, uint8_t * key, size_t key_len); +// Get the value of a key in the map into data +void map_get(Map * map, uint8_t * key, size_t key_len, void * data); +// Remove a key value pair from the map, if data is not NULL, write value to it. +void map_remove(Map * map, uint8_t * key, size_t key_len, void * data); +// Get an iterator of the map +MapIter map_iter(Map * map); +// Test if there is a next +bool map_iter_has_next(MapIter * iter); +// Get the next value, if key is not NULL, put a pointer to the key in key, if key_len is not NULL +// put the len of the key in key_len, if data is not null, put a pointer to the data in data. +void map_iter_next(MapIter * iter, void ** key, size_t * key_len, void ** data); + +// map_insert with a string key +static inline void map_insert_str(Map * map, const char * key, void * data) { + size_t len = strlen(key) + 1; + map_insert(map, (void *)key, len, data); +} +// map_contains with a string key +static inline bool map_contains_str(Map * map, const char * key) { + size_t len = strlen(key) + 1; + return map_contains(map, (void *)key, len); +} +// map_get with a string key +static inline void map_get_str(Map * map, const char * key, void * data) { + size_t len = strlen(key) + 1; + map_get(map, (void *)key, len, data); +} +// map_remove with a string key +static inline void map_remove_str(Map * map, const char * key, void * data) { + size_t len = strlen(key) + 1; + map_remove(map, (void *)key, len, data); +} + +void map_debug(Map map, char *format); #endif diff --git a/hid.c b/hid.c index dc86bae..d0fac66 100644 --- a/hid.c +++ b/hid.c @@ -1,17 +1,236 @@ -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -void hid_main() { - DIR *d; - struct dirent *dir; - d = opendir("/sys/class/hidraw"); - if(d) { - while ((dir = readdir(d)) != NULL) { - if(dir->d_type != DT_LNK) continue; - printf("%s\n", dir->d_ino); +#include "hid.h" +#include "vec.h" +#include "main.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; +// Mutex for devices +static pthread_mutex_t devices_mutex = PTHREAD_MUTEX_INITIALIZER; + +// uniqs are just hexadecimal numbers with colons in between each byte +uniq_t parse_uniq(char uniq[17]) { + uniq_t res = 0; + for(int i = 0; i < 17; i++) { + char c = uniq[i]; + int digit; + 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 continue; + res <<= 4; + res += digit; } - closedir(d); - } - exit(0); + return res; +} + +bool filter_event(int fd, char * event) { + char device_path[64]; + snprintf(device_path, 64, "/sys/class/input/%s/device", event); + + DIR * device_dir = opendir(device_path); + struct dirent * device_dirent; + + bool found = false; + while((device_dirent = readdir(device_dir)) != NULL) { + if(device_dirent->d_type == DT_DIR && strncmp(device_dirent->d_name, "js", 2) == 0) { + found = true; + break; + } + } + + if(!found) { + return false; + } + + uint16_t info[4]; + ioctl(fd, EVIOCGID, info); + return info[1] == 0x054c && info[2] == 0x05c4; +} + +void poll_devices_init() { + devices = vec_of(uniq_t); + new_devices = vec_of(PhysicalDevice); + devices_queue = vec_of(PhysicalDevice); +} + +PhysicalDevice get_device() { + 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; + } + while(devices_queue.len == 0) { + pthread_cond_wait(&devices_queue_cond, &devices_queue_mutex); + } + + PhysicalDevice res; + vec_pop(&devices_queue, &res); + if(devices_queue.len > 0) { + pthread_cond_signal(&devices_queue_cond); + } + pthread_mutex_unlock(&devices_queue_mutex); + return res; +} + +void return_device(PhysicalDevice * dev) { + close(dev->event); + close(dev->hidraw); + 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); + break; + } + } + pthread_mutex_unlock(&devices_mutex); +} + +void poll_devices() { + vec_clear(&new_devices); + + DIR * input_dir = opendir("/sys/class/input"); + struct dirent * input; + while((input = readdir(input_dir)) != NULL) { + // Ignore if the entry isn't a linkg or doesn't start with event + if(input->d_type != DT_LNK || strncmp(input->d_name, "event", 5) != 0) { + continue; + } + + PhysicalDevice dev = {}; + + char event_path[64]; + snprintf(event_path, 64, "/dev/input/%s", input->d_name); + + dev.event = open(event_path, O_RDONLY); + + if(dev.event < 0) { + continue; + } + + char name[256] = {}; + ioctl(dev.event, EVIOCGNAME(256), name); + + if(!filter_event(dev.event, input->d_name)) goto skip; + + uniq_t uniq; + { + char uniq_str[17] = {}; + + char uniq_path[256]; + snprintf(uniq_path, 256, "/sys/class/input/%s/device/uniq", input->d_name); + + int uniq_fd = open(uniq_path, O_RDONLY); + + if(uniq_fd < 0) goto skip; + + read(uniq_fd, uniq_str, 17); + uniq = parse_uniq(uniq_str); + + close(uniq_fd); + + // If we couldn't parse the uniq (this assumes uniq can't be zero, which is probably alright) + if(uniq == 0) goto skip; + } + + bool found = false; + + pthread_mutex_lock(&devices_mutex); + for(int i = 0; i < devices.len; i++) { + uniq_t * dev_uniq = vec_get(&devices, i); + if(*dev_uniq == uniq) { + found = true; + break; + } + } + pthread_mutex_unlock(&devices_mutex); + + if(found) goto skip; + + dev.uniq = uniq; + + char hidraw_path[64]; + + { + char hidraw_path[256]; + snprintf(hidraw_path, 256, "/sys/class/input/%s/device/device/hidraw", input->d_name); + + DIR * hidraw_dir = opendir(hidraw_path); + struct dirent * hidraw = NULL; + while((hidraw = readdir(hidraw_dir)) != NULL) { + if(strncmp(hidraw->d_name, "hidraw", 6) == 0) break; + } + + if(hidraw == NULL) { + printf("Couldn't get hidraw of %s", input->d_name); + continue; + } + + snprintf(hidraw_path, 64, "/dev/%s", hidraw->d_name); + + closedir(hidraw_dir); + } + + dev.hidraw = open(hidraw_path, O_WRONLY); + if(dev.hidraw < 0) goto skip; + + pthread_mutex_lock(&devices_mutex); + vec_push(&devices, &uniq); + 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; + + // close open file descriptor and continue +skip: + close(dev.event); + continue; + }; + + closedir(input_dir); + 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); + } +} + +void * hid_thread() { + printf("HID: start\n"); + poll_devices_init(); + while(1) { + poll_devices(); + struct timespec ts; + ts.tv_sec = 1; + ts.tv_nsec = 0; + nanosleep(&ts, NULL); + } + return NULL; } diff --git a/hid.h b/hid.h index c987e44..77bae69 100644 --- a/hid.h +++ b/hid.h @@ -1,7 +1,19 @@ #ifndef HID_H #define HID_H +#include +#include +#include "vec.h" +typedef uint64_t uniq_t; -void hid_main(); +typedef struct { + int event; + int hidraw; + uniq_t uniq; +} PhysicalDevice; + +void * hid_thread(); +void return_device(PhysicalDevice * dev); +PhysicalDevice get_device(); #endif diff --git a/main.c b/main.c index c93e338..cdedd82 100644 --- a/main.c +++ b/main.c @@ -1,29 +1,24 @@ -// vi: set shiftwidth=4 : set softtabstop=4 -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include "main.h" #include "hid.h" +#include "server.h" const char* USAGE[] = { - "jsfw client [input] [address] [port]\n", + "jsfw client [address] [port]\n", "jsfw server [port]\n", }; const size_t EVENT_SIZE = sizeof(struct js_event); -void joystick_debug(Joystick * js) { - printf("Joystick"); - if(js->name) printf(" (%s)", js->name); - printf(": %u buttons, %u axes\n", js->button_count, js->axis_count); -} - void panicf(const char *fmt, ...) { va_list args; va_start(args, fmt); @@ -34,49 +29,22 @@ void panicf(const char *fmt, ...) { 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 0..%d, got %lld\n", UINT16_MAX, n); + 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) { printf("Server (port: %u).\n", port); - panicf("Uninplemented\n"); + + pthread_t _; + pthread_create(&_, NULL, hid_thread, NULL); + + server_run(port); } -void client(char * input, char * address, uint16_t port) { - hid_main(); - printf("JSFW Client (%s -> %s:%d)\n", input, address, port); - int fd = open(input, O_RDONLY); - if(fd < 0) panicf("Couldn't open %s", input); - - Joystick js = {}; - - char name[256]; - int name_len = ioctl(fd, JSIOCGNAME(256), name); - if(name_len >= 0) { - js.name = malloc(name_len); - if(js.name) strncpy(js.name, name, name_len); - } - - ioctl(fd, JSIOCGBUTTONS, &js.button_count); - ioctl(fd, JSIOCGAXES, &js.axis_count); - - joystick_debug(&js); - - struct js_event events[128]; - while(1) { - int bytes = read(fd, events, EVENT_SIZE); - if(bytes < EVENT_SIZE) { - printf("Got %d bytes, expected at least %lu", bytes, EVENT_SIZE); - continue; - } - int count = bytes / EVENT_SIZE; - for(int i = 0; i < count; i++) { - struct js_event event = events[i]; - printf("EV | type(%d) number(%d) value(%d) ts(%d)\n", event.type, event.number, event.value, event.time); - } - } +void client(char * address, uint16_t port) { + printf("JSFW Client (%s:%d)\n", address, port); } int main(int argc, char* argv[]) { @@ -98,13 +66,12 @@ int main(int argc, char* argv[]) { } else if(strcmp(mode, "client") == 0) { - if(argc < 5) + if(argc < 4) panicf("Usage: %s", USAGE[0]); - char * input = argv[2]; - char * address = argv[3]; - uint16_t port = parse_port(argv[4]); - client(input, address, port); + char * address = argv[2]; + uint16_t port = parse_port(argv[3]); + client(address, port); } else { printf("Unknown mode: '%s'\n", mode); diff --git a/main.h b/main.h index f7d68fc..dd67f7c 100644 --- a/main.h +++ b/main.h @@ -1,11 +1,9 @@ #ifndef MAIN_H #define MAIN_H -#include +#include +#include +#include "vec.h" -typedef struct { - char * name; - uint8_t button_count; - uint8_t axis_count; -} Joystick; +void panicf(const char * fmt, ...); #endif diff --git a/net.c b/net.c index e69de29..c45a72e 100644 --- a/net.c +++ b/net.c @@ -0,0 +1,87 @@ +#include "net.h" + +Message msg_device_info() { + MessageDeviceInfo m; + m.code = DeviceInfo; + + Message s; + s.device_info = m; + return s; +} + +int msg_deserialize(const uint8_t * buf, size_t len, Message * dst) { + // 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; + + switch(code) { + case Heartbeat: + if(MSS_HEARTBEAT > len) return -1; + dst->code = code; + dst->heartbeat.alive = buf[1]; + return 0; + case DeviceInfo: + if(MSS_DEVICE_INFO > len) return -1; + dst->code = code; + return 0; + case DeviceReport: + if(len < MSS_DEVICE_REPORT) return -1; + dst->code = code; + return 0; + case DeviceDestroy: + if(len < MSS_DEVICE_DESTROY) return -1; + dst->code = code; + return 0; + 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; + default: + return -1; + } +} + +// The indices have to match with msg_deserialize +int msg_serialize(uint8_t * buf, size_t len, Message msg) { + switch(msg.code) { + case Heartbeat: + if(MSS_HEARTBEAT >= len) return -1; + buf[0] = (uint8_t) msg.code; + buf[1] = msg.heartbeat.alive; + return 0; + case DeviceInfo: + if(MSS_DEVICE_INFO >= len) return -1; + buf[0] = (uint8_t) msg.code; + return 0; + case DeviceReport: + if(MSS_DEVICE_REPORT >= len) return -1; + buf[0] = (uint8_t) msg.code; + return 0; + case DeviceDestroy: + if(MSS_DEVICE_DESTROY >= len) return -1; + buf[0] = (uint8_t) msg.code; + return 0; + case ControllerState: + if(MSS_CONTROLLER_STATE >= len) 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 0; + default: + return -1; + } +} diff --git a/net.h b/net.h index 4765a53..da5df48 100644 --- a/net.h +++ b/net.h @@ -1,4 +1,59 @@ #ifndef NET_H #define NET_H +#include +#include + +typedef enum { + Heartbeat = 0, + DeviceInfo = 1, + DeviceReport = 2, + DeviceDestroy = 3, + ControllerState = 4, +} MessageCode; + +typedef struct { + MessageCode code; + uint8_t alive; +} MessageHeartbeat; +#define MSS_HEARTBEAT 1 +// MSS -> Message Serialized Size: +// Size of the data of the message when serialized (no alignment / padding) + +typedef struct { + MessageCode code; +} MessageDeviceInfo; +#define MSS_DEVICE_INFO 0 + +typedef struct { + MessageCode code; +} MessageDeviceReport; +#define MSS_DEVICE_REPORT 0 + +typedef struct { + MessageCode code; +} MessageDeviceDestroy; +#define MSS_DEVICE_DESTROY 0 + +typedef struct { + MessageCode code; + 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 + +typedef union { + MessageCode code; + MessageHeartbeat heartbeat; + MessageDeviceInfo device_info; + MessageDeviceReport device_report; + MessageDeviceDestroy device_destroy; + MessageControllerState controller_state; +} Message; + +int msg_deserialize(const uint8_t * buf, size_t len, Message * dst); +int msg_serialize(uint8_t * buf, size_t len, Message msg); #endif diff --git a/server.c b/server.c new file mode 100644 index 0000000..58a40d5 --- /dev/null +++ b/server.c @@ -0,0 +1,75 @@ +#include +#include +#include +#include +#include +#include + +#include "main.h" +#include "vec.h" +#include "net.h" + +struct Connection { + int socket; + uint32_t id; +}; + +void * server_handle_conn(void * args_) { + struct Connection * args = args_; + + printf("THREAD(%u): start\n", args->id); + while(1) { + uint8_t buf[1024]; + while(1) { + int len = recv(args->socket, buf, 1024, MSG_WAITALL); + if(len <= 0) break; + Message msg; + if(msg_deserialize(buf, len, &msg) == 0) { + + } else { + printf("Couldn't parse message.\n"); + } + } + printf("THREAD(%u): connection closed\n", args->id); + } + + free(args); + return NULL; +} + +void server_run(uint16_t port) { + printf("SERVER: start\n"); + int sock = socket(AF_INET, SOCK_STREAM, 0); + if(sock < 0) panicf("Couldn't open socket\n"); + struct sockaddr_in addr = {}; + addr.sin_family = AF_INET; + addr.sin_addr.s_addr = htonl(INADDR_ANY); + addr.sin_port = htons(port); + if(bind(sock, (struct sockaddr*)&addr, sizeof(addr)) != 0) + panicf("Couldn't bind to the socket\n"); + if(listen(sock, 16) != 0) + panicf("Couldn't listen on socket\n"); + + uint32_t ids = 0; + while(1) { + struct sockaddr con_addr; + socklen_t con_len = sizeof(con_addr); + struct Connection conn; + + conn.socket = accept(sock, &con_addr, &con_len); + + if(conn.socket >= 0) { + printf("SERVER: got connection\n"); + + conn.id = ids++; + + struct Connection * conn_ptr = malloc(sizeof(struct Connection)); + memcpy(conn_ptr, &conn, sizeof(struct Connection)); + + pthread_t thread; + pthread_create(&thread, NULL, server_handle_conn, conn_ptr); + } else { + printf("Couldn't accept connection (%d)\n", conn.socket); + } + } +} diff --git a/server.h b/server.h new file mode 100644 index 0000000..469f00e --- /dev/null +++ b/server.h @@ -0,0 +1,7 @@ +#ifndef SERVER_H +#define SERVER_H +#include + +void server_run(uint16_t port); + +#endif diff --git a/util.c b/util.c new file mode 100644 index 0000000..2a93b05 --- /dev/null +++ b/util.c @@ -0,0 +1,22 @@ +#include "util.h" +#include + +#ifndef __has_builtin +#define __has_builtin(_) 0 +#endif + +unsigned long log2lu(unsigned long n) { +#if __has_builtin(__builtin_clz) + return sizeof(unsigned long) * CHAR_BIT - __builtin_clz(n) - 1; +#else + unsigned long res = 0; + while(n >>= 1) ++res; + return res; +#endif +} + +uint32_t rotl(uint32_t n, unsigned int c) { + const unsigned int mask = (CHAR_BIT*sizeof(n) - 1); + c &= mask; + return (n<>( (-c)&mask )); +} diff --git a/util.h b/util.h new file mode 100644 index 0000000..c2c10db --- /dev/null +++ b/util.h @@ -0,0 +1,8 @@ +#ifndef UTIL_H +#define UTIL_H +#include + +unsigned long log2lu(unsigned long); +uint32_t rotl (uint32_t n, unsigned int c); + +#endif diff --git a/vec.c b/vec.c new file mode 100644 index 0000000..82049bb --- /dev/null +++ b/vec.c @@ -0,0 +1,100 @@ +#include +#include +#include +#include "vec.h" + +#define INIT_CAP 8 + +static void handle_alloc_error() { + printf("Error when allocating memory.\n"); + exit(2); +} + +Vec vec_new(size_t data_size) { + return vec_cap(data_size, INIT_CAP); +} + +Vec vec_cap(size_t data_size, size_t initial_capacity) { + Vec v; + v.cap = initial_capacity; + v.len = 0; + v.stride = data_size; + v.data = malloc(data_size * initial_capacity); + return v; +} + +static inline void vec_grow(Vec * v, size_t cap) { + if(v->cap >= cap) return; + size_t new_cap = cap > v->cap * 2 ? cap : v->cap * 2; + void * new_data = realloc(v->data, new_cap * v->stride); + if(new_data == NULL) handle_alloc_error(); + v->data = new_data; + v->cap = new_cap; +} + +void vec_push(Vec * v, void * data) { + vec_grow(v, v->len + 1); + memcpy(v->data + v->stride * v->len++, data, v->stride); +} + +void vec_pop(Vec * v, void * data) { + if(v->len == 0) { + printf("ERR(vec_pop): Trying to pop an element from an empty vector\n"); + return; + } + if(data != NULL) + memcpy(data, v->data + v->stride * (v->len - 1), v->stride); + v->len--; +} + +void * vec_get(Vec * v, size_t index) { + if(index >= v->len) return NULL; + return v->data + index * v->stride; +} + +void vec_insert(Vec * v, void * data, size_t index) { + if(index > v->len) { + printf("ERR(vec_insert): Trying to insert past the end of the vector.\n"); + return; + } + vec_grow(v, v->len + 1); + + void * slot = v->data + index * v->stride; + if(index < v->len) { + memmove(slot + v->stride, slot, (v->len - index) * v->stride); + } + memcpy(slot, data, v->stride); + v->len++; +} + +void vec_remove(Vec * v, size_t index, void * data) { + if(v->len == 0) { + printf("ERR(vec_remove): Trying to remove an element from an empty vector\n"); + return; + } + if(index >= v->len) { + printf("ERR(vec_remove): Trying to remove past the end of the vector\n"); + return; + } + + void * slot = v->data + index * v->stride; + if(data != NULL) + memcpy(data, slot, v->stride); + if(index < --v->len) + memmove(slot, slot + v->stride, (v->len - index) * v->stride); +} + +void vec_clear(Vec * v) { + v->len = 0; +} + +void vec_extend(Vec * v, void * data, size_t len) { + if(len == 0) return; + vec_grow(v, v->len + len); + memcpy(v->data + v->stride * v->len, data, v->stride * len); + v->len += len; +} + +void vec_free(Vec v) { + free(v.data); +} diff --git a/vec.h b/vec.h new file mode 100644 index 0000000..19b65d1 --- /dev/null +++ b/vec.h @@ -0,0 +1,36 @@ +#ifndef VEC_H +#define VEC_H +#include +#include + +#define vec_of(type) vec_new(sizeof(type)) + +typedef struct { + void * data; + size_t cap; + size_t len; + size_t stride; +} Vec; + +// Create a new vector +Vec vec_new(size_t data_size); +// Create a new vector with an initial capacity +Vec vec_cap(size_t data_size, size_t initial_capacity); +// Push an element into the vector +void vec_push(Vec * v, void * data); +// Pop an element into the vector, and put it in data if it is not null +void vec_pop(Vec * v, void * data); +// Get a pointer to the element at an index, returns NULL if there is no such element +void * vec_get(Vec * v, size_t index); +// Insert an element at any index in the vector (except past the end) +void vec_insert(Vec * v, void * data, size_t index); +// Remove an element from the vector, and put it in data if it is not NULL +void vec_remove(Vec * v, size_t index, void * data); +// Clear the vector +void vec_clear(Vec * v); +// Extend the vector with the content of data +void vec_extend(Vec * v, void * data, size_t len); +// Free the vector +void vec_free(Vec v); + +#endif