diff --git a/Makefile b/Makefile index ff0de7f..65a9c05 100644 --- a/Makefile +++ b/Makefile @@ -1,7 +1,7 @@ CFLAGS= -g -O0 -D _DEFAULT_SOURCE -D_POSIX_C_SOURCE=200112L -std=c11 -fPIE -pedantic -Wall # -Werror -LDFLAGS=-lpthread -levdev -lrt +LDFLAGS=-lpthread -levdev -lrt -lm CC=gcc -OBJECTS=main.o input_dev.o output_dev.o queue.o +OBJECTS=main.o input_dev.o dev_iio.o output_dev.o queue.o TARGET=rogue_enemy all: $(TARGET) diff --git a/dev_iio.c b/dev_iio.c new file mode 100644 index 0000000..e945953 --- /dev/null +++ b/dev_iio.c @@ -0,0 +1,211 @@ +#include "dev_iio.h" +#include + +dev_iio_t* dev_iio_create(const char* path) { + dev_iio_t *const iio = malloc(sizeof(dev_iio_t)); + iio->anglvel_x = -1; + iio->anglvel_y = -1; + iio->anglvel_z = -1; + iio->accel_x = -1; + iio->accel_y = -1; + iio->accel_z = -1; + + iio->accel_scale_x = 0.0f; + iio->accel_scale_y = 0.0f; + iio->accel_scale_z = 0.0f; + iio->anglvel_scale_x = 0.0f; + iio->anglvel_scale_y = 0.0f; + iio->anglvel_scale_z = 0.0f; + + iio->outer_accel_scale_x = ACCEL_SCALE; + iio->outer_accel_scale_y = ACCEL_SCALE; + iio->outer_accel_scale_z = ACCEL_SCALE; + iio->outer_anglvel_scale_x = GYRO_SCALE; + iio->outer_anglvel_scale_y = GYRO_SCALE; + iio->outer_anglvel_scale_z = GYRO_SCALE; + + return iio; +} + +void dev_iio_destroy(dev_iio_t* iio) { + free(iio->name); + free(iio->path); + free(iio->accel_scale_x_fd); + free(iio->accel_scale_y_fd); + free(iio->accel_scale_z_fd); + free(iio->anglvel_scale_x_fd); + free(iio->anglvel_scale_y_fd); + free(iio->anglvel_scale_z_fd); +} + +const char* dev_iio_get_name(const dev_iio_t* iio) { + return iio->name; +} + +const char* dev_iio_get_path(const dev_iio_t* iio) { + return iio->path; +} + +int dev_iio_read( + const dev_iio_t *const iio, + struct input_event *const buf, + size_t buf_sz, + uint32_t *const buf_out +) { + *buf_out = 0; + char tmp[128]; + + struct timeval now = {0}; + + if (*buf_out == buf_sz) { + return -ENOMEM; + } else if (iio->accel_x != -1) { + memset((void*)&tmp[0], 0, sizeof(tmp)); + int tmp_read = read(iio->accel_x, (void*)&tmp[0], sizeof(tmp)); + if (tmp_read >= 0) { + gettimeofday(&now, NULL); + const double val = strtod(&tmp[0], NULL); + const double val_in_m2s = val * iio->accel_scale_x; + + struct input_event* ev = &buf[*buf_out]; + + ev->time = now; + ev->type = EV_ABS; + ev->code = ABS_X; + ev->value = (__s32)(val_in_m2s * iio->outer_accel_scale_x); + + // TODO: seek + } else { + return tmp_read; + } + + ++(*buf_out); + } + + if (*buf_out == buf_sz) { + return -ENOMEM; + } else if (iio->accel_y != -1) { + memset((void*)&tmp[0], 0, sizeof(tmp)); + int tmp_read = read(iio->accel_y, (void*)&tmp[0], sizeof(tmp)); + if (tmp_read >= 0) { + gettimeofday(&now, NULL); + const double val = strtod(&tmp[0], NULL); + const double val_in_m2s = val * iio->accel_scale_y; + + struct input_event* ev = &buf[*buf_out]; + + ev->time = now; + ev->type = EV_ABS; + ev->code = ABS_Y; + ev->value = (__s32)(val_in_m2s * iio->outer_accel_scale_y); + + // TODO: seek + } else { + return tmp_read; + } + + ++(*buf_out); + } + + if (*buf_out == buf_sz) { + return -ENOMEM; + } else if (iio->accel_z != -1) { + memset((void*)&tmp[0], 0, sizeof(tmp)); + int tmp_read = read(iio->accel_z, (void*)&tmp[0], sizeof(tmp)); + if (tmp_read >= 0) { + gettimeofday(&now, NULL); + const double val = strtod(&tmp[0], NULL); + const double val_in_m2s = val * iio->accel_scale_z; + + struct input_event* ev = &buf[*buf_out]; + + ev->time = now; + ev->type = EV_ABS; + ev->code = ABS_Z; + ev->value = (__s32)(val_in_m2s * iio->outer_accel_scale_z); + + // TODO: seek + } else { + return tmp_read; + } + + ++(*buf_out); + } + + if (*buf_out == buf_sz) { + return -ENOMEM; + } else if (iio->anglvel_x != -1) { + memset((void*)&tmp[0], 0, sizeof(tmp)); + int tmp_read = read(iio->anglvel_x, (void*)&tmp[0], sizeof(tmp)); + if (tmp_read >= 0) { + gettimeofday(&now, NULL); + const double val = strtod(&tmp[0], NULL); + const double val_in_m2s = val * iio->anglvel_scale_x; + + struct input_event* ev = &buf[*buf_out]; + + ev->time = now; + ev->type = EV_ABS; + ev->code = ABS_RX; + ev->value = (__s32)(val_in_m2s * iio->outer_anglvel_scale_x); + + // TODO: seek + } else { + return tmp_read; + } + + ++(*buf_out); + } + + if (*buf_out == buf_sz) { + return -ENOMEM; + } else if (iio->anglvel_y != -1) { + memset((void*)&tmp[0], 0, sizeof(tmp)); + int tmp_read = read(iio->anglvel_y, (void*)&tmp[0], sizeof(tmp)); + if (tmp_read >= 0) { + gettimeofday(&now, NULL); + const double val = strtod(&tmp[0], NULL); + const double val_in_m2s = val * iio->anglvel_scale_y; + + struct input_event* ev = &buf[*buf_out]; + + ev->time = now; + ev->type = EV_ABS; + ev->code = ABS_RY; + ev->value = (__s32)(val_in_m2s * iio->outer_anglvel_scale_y); + + // TODO: seek + } else { + return tmp_read; + } + + ++(*buf_out); + } + + if (*buf_out == buf_sz) { + return -ENOMEM; + } else if (iio->anglvel_z != -1) { + memset((void*)&tmp[0], 0, sizeof(tmp)); + int tmp_read = read(iio->anglvel_z, (void*)&tmp[0], sizeof(tmp)); + if (tmp_read >= 0) { + gettimeofday(&now, NULL); + const double val = strtod(&tmp[0], NULL); + const double val_in_m2s = val * iio->anglvel_scale_z; + + struct input_event* ev = &buf[*buf_out]; + + ev->time = now; + ev->type = EV_ABS; + ev->code = ABS_RZ; + ev->value = (__s32)(val_in_m2s * iio->outer_anglvel_scale_z); + + // TODO: seek + } else { + return tmp_read; + } + + ++(*buf_out); + } + + return 0; +} diff --git a/dev_iio.h b/dev_iio.h new file mode 100644 index 0000000..35e6544 --- /dev/null +++ b/dev_iio.h @@ -0,0 +1,71 @@ +#pragma once + +#include "rogue_enemy.h" + +#define DEV_IIO_HAS_ACCEL 0x00000001U +#define DEV_IIO_HAS_ANGLVEL 0x00000002U + +#define ACCEL_SCALE ((double)(255.0)/(double)(9.81)) // convert m/s^2 to g's, and scale x255 to increase precision when passed to evdev as an int +#define GYRO_SCALE ((double)(180.0)/(double)(M_PI)) // convert radians/s to degrees/s + +typedef struct dev_iio { + char* path; + char* name; + uint32_t flags; + + char* accel_scale_x_fd; + char* accel_scale_y_fd; + char* accel_scale_z_fd; + + int accel_x; + int accel_y; + int accel_z; + + double accel_scale_x; + double accel_scale_y; + double accel_scale_z; + + char* anglvel_scale_x_fd; + char* anglvel_scale_y_fd; + char* anglvel_scale_z_fd; + + double outer_accel_scale_x; + double outer_accel_scale_y; + double outer_accel_scale_z; + + int anglvel_x; + int anglvel_y; + int anglvel_z; + + double anglvel_scale_x; + double anglvel_scale_y; + double anglvel_scale_z; + + double outer_anglvel_scale_x; + double outer_anglvel_scale_y; + double outer_anglvel_scale_z; + +} dev_iio_t; + +dev_iio_t* dev_iio_create(const char* path); + +void dev_iio_destroy(dev_iio_t* iio); + +const char* dev_iio_get_name(const dev_iio_t* iio); + +const char* dev_iio_get_path(const dev_iio_t* iio); + +inline int dev_iio_has_anglvel(const dev_iio_t* iio) { + return (iio->flags & DEV_IIO_HAS_ANGLVEL) != 0; +} + +inline int dev_iio_has_accel(const dev_iio_t* iio) { + return (iio->flags & DEV_IIO_HAS_ACCEL) != 0; +} + +int dev_iio_read( + const dev_iio_t *const iio, + struct input_event *const buf, + size_t buf_sz, + uint32_t *const buf_out +); \ No newline at end of file diff --git a/input_dev.c b/input_dev.c index bb3e666..28f08f2 100644 --- a/input_dev.c +++ b/input_dev.c @@ -1,6 +1,7 @@ #include "input_dev.h" #include "message.h" #include "queue.h" +#include "dev_iio.h" #include #include @@ -15,6 +16,7 @@ #include static const char *input_path = "/dev/input/"; +static const char *iio_path = "/sys/bus/iio/devices/"; static struct libevdev* ev_matches(const char* sysfs_entry, const uinput_filters_t* const filters) { struct libevdev *dev = NULL; @@ -50,6 +52,20 @@ static struct libevdev* ev_matches(const char* sysfs_entry, const uinput_filters return dev; } +static dev_iio_t* iio_matches(const char* sysfs_entry, const iio_filters_t* const filters) { + dev_iio_t *const dev_iio = dev_iio_create(sysfs_entry); + if (dev_iio == NULL) { + return NULL; + } + + if (strcmp(dev_iio_get_name(dev_iio), filters->name) != 0) { + dev_iio_destroy(dev_iio); + return NULL; + } + + return dev_iio; +} + static pthread_mutex_t input_acquire_mutex = PTHREAD_MUTEX_INITIALIZER; static char* open_sysfs[] = { @@ -61,16 +77,65 @@ static char* open_sysfs[] = { }; #define MAX_MESSAGES_IN_FLIGHT 32 -#define DEFAULT_EVENTS_IN_REPORT 4 +#define DEFAULT_EVENTS_IN_REPORT 8 struct input_ctx { struct libevdev* dev; + dev_iio_t *iio_dev; queue_t* queue; message_t messages[MAX_MESSAGES_IN_FLIGHT]; }; -void* input_read_thread_func(void* ptr) { +static void* iio_read_thread_func(void* ptr) { + struct input_ctx* ctx = (struct input_ctx*)ptr; + + message_t* msg = NULL; + + int rc = -1; + + do { + if (msg == NULL) { + for (int h = 0; h < MAX_MESSAGES_IN_FLIGHT; ++h) { + if ((ctx->messages[h].flags & MESSAGE_FLAGS_HANDLE_DONE) != 0) { + msg = &ctx->messages[h]; + msg->ev_count = 0; + break; + } + } + } + + if (msg == NULL) { + fprintf(stderr, "Events are stalled.\n"); + continue; + } + + const int res = dev_iio_read(ctx->iio_dev, msg->ev, msg->ev_size, &msg->ev_count); + if (res == 0) { + + } else if (res == -ENOMEM) { + fprintf(stderr, "Error: out-of-memory will skip the current frame.\n"); + continue; + } + + // clear out flags + msg->flags = 0x00000000U; + + if (queue_push(ctx->queue, (void*)msg) != 0) { + fprintf(stderr, "Error pushing event.\n"); + + // flag the memory to be safe to reuse + msg->flags |= MESSAGE_FLAGS_HANDLE_DONE; + } + + // either way.... fill a new buffer on the next cycle + msg = NULL; + } while (rc == 1 || rc == 0 || rc == -EAGAIN); + + return NULL; +} + +static void* input_read_thread_func(void* ptr) { struct input_ctx* ctx = (struct input_ctx*)ptr; struct libevdev* dev = ctx->dev; @@ -101,6 +166,11 @@ void* input_read_thread_func(void* ptr) { if (rc == 0) { const int is_syn = (read_ev.type == EV_SYN) && (read_ev.code == SYN_REPORT); + if ((read_ev.code == MSC_TIMESTAMP) && (read_ev.type = EV_MSC)) { + // the output device will handle that + continue; + } + if ((!has_syn) || ((has_syn) && (!is_syn))) { if ((msg->ev_count+1) == msg->ev_size) { // TODO: perform a memove @@ -147,20 +217,10 @@ void* input_read_thread_func(void* ptr) { return NULL; } -void *input_dev_thread_func(void *ptr) { - input_dev_t *in_dev = (input_dev_t*)ptr; - - struct input_ctx ctx = { - .dev = NULL, - .queue = in_dev->queue, - }; - - for (int h = 0; h < MAX_MESSAGES_IN_FLIGHT; ++h) { - ctx.messages[h].flags = MESSAGE_FLAGS_HANDLE_DONE; - ctx.messages[h].ev_size = DEFAULT_EVENTS_IN_REPORT; - ctx.messages[h].ev = malloc(sizeof(struct input_event) * ctx.messages[h].ev_size); - } - +static void input_iio( + input_dev_t *const in_dev, + struct input_ctx *const ctx +) { int open_sysfs_idx = -1; for (;;) { @@ -171,9 +231,116 @@ void *input_dev_thread_func(void *ptr) { } // clean up from previous iteration - if (ctx.dev != NULL) { - libevdev_free(ctx.dev); - ctx.dev = NULL; + if (ctx->iio_dev != NULL) { + dev_iio_destroy(ctx->iio_dev); + ctx->dev = NULL; + } + + const int input_acquire_lock_result = pthread_mutex_lock(&input_acquire_mutex); + if (input_acquire_lock_result != 0) { + fprintf(stderr, "Cannot lock input mutex: %d, will retry later...\n", input_acquire_lock_result); + usleep(250000); + continue; + } + + // clean up leftover from previous opening + if (open_sysfs_idx >= 0) { + free(open_sysfs[open_sysfs_idx]); + open_sysfs[open_sysfs_idx] = NULL; + } + + char path[512] = "\0"; + + DIR *d; + struct dirent *dir; + d = opendir(iio_path); + if (d) { + while ((dir = readdir(d)) != NULL) { + if (dir->d_name[0] == '.') { + continue; + } else if (dir->d_name[0] == 'b') { // by-id + continue; + } else if (dir->d_name[0] == 'j') { // js-0 + continue; + } + + sprintf(path, "%s%s", input_path, dir->d_name); + + // check if that has been already opened + // open_sysfs + int skip = 0; + for (int o = 0; o < (sizeof(open_sysfs) / sizeof(const char*)); ++o) { + if ((open_sysfs[o] != NULL) && (strcmp(open_sysfs[o], path) == 0)) { + skip = 1; + break; + } + } + + if (skip) { + continue; + } + + // try to open the device + ctx->iio_dev = iio_matches(path, in_dev->iio_filters); + if (ctx->dev != NULL) { + open_sysfs_idx = 0; + while (open_sysfs[open_sysfs_idx] != NULL) { + ++open_sysfs_idx; + } + open_sysfs[open_sysfs_idx] = malloc(sizeof(path)); + memcpy(open_sysfs[open_sysfs_idx], path, 512); + + // TODO: populate ctx->iio_path + + printf("Opened device %s\n name: %s\n rumble: no EV_FF\n", + path, + dev_iio_get_name(ctx->iio_dev) + ); + + break; + } + } + closedir(d); + } + + pthread_mutex_unlock(&input_acquire_mutex); + + // TODO: if device was not open "continue" + if (ctx->iio_dev == NULL) { + usleep(250000); + continue; + } + + pthread_t incoming_events_thread; + + const int incoming_events_thread_creation = pthread_create(&incoming_events_thread, NULL, iio_read_thread_func, (void*)&ctx); + if (incoming_events_thread_creation != 0) { + fprintf(stderr, "Error creating the input thread for device %s: %d\n", dev_iio_get_name(ctx->iio_dev), incoming_events_thread_creation); + } + + if (incoming_events_thread_creation == 0) { + pthread_join(incoming_events_thread, NULL); + } + } +} + +static void input_udev( + input_dev_t *const in_dev, + struct input_ctx *const ctx +) { + int open_sysfs_idx = -1; + + for (;;) { + const uint32_t flags = in_dev->crtl_flags; + if (flags & INPUT_DEV_CTRL_FLAG_EXIT) { + in_dev->crtl_flags &= ~INPUT_DEV_CTRL_FLAG_EXIT; + break; + } + + // clean up from previous iteration + if (ctx->dev != NULL) { + libevdev_free(ctx->dev); + ctx->dev = NULL; } const int input_acquire_lock_result = pthread_mutex_lock(&input_acquire_mutex); @@ -221,8 +388,8 @@ void *input_dev_thread_func(void *ptr) { } // try to open the device - ctx.dev = ev_matches(path, in_dev->ev_filters); - if (ctx.dev != NULL) { + ctx->dev = ev_matches(path, in_dev->ev_filters); + if (ctx->dev != NULL) { open_sysfs_idx = 0; while (open_sysfs[open_sysfs_idx] != NULL) { ++open_sysfs_idx; @@ -230,16 +397,16 @@ void *input_dev_thread_func(void *ptr) { open_sysfs[open_sysfs_idx] = malloc(sizeof(path)); memcpy(open_sysfs[open_sysfs_idx], path, 512); - if (libevdev_has_event_type(ctx.dev, EV_FF)) { + if (libevdev_has_event_type(ctx->dev, EV_FF)) { printf("Opened device %s\n name: %s\n rumble: %s\n", path, - libevdev_get_name(ctx.dev), - libevdev_has_event_code(ctx.dev, EV_FF, FF_RUMBLE) ? "true" : "false" + libevdev_get_name(ctx->dev), + libevdev_has_event_code(ctx->dev, EV_FF, FF_RUMBLE) ? "true" : "false" ); } else { printf("Opened device %s\n name: %s\n rumble: no EV_FF\n", path, - libevdev_get_name(ctx.dev) + libevdev_get_name(ctx->dev) ); } @@ -251,7 +418,7 @@ void *input_dev_thread_func(void *ptr) { pthread_mutex_unlock(&input_acquire_mutex); - if (ctx.dev == NULL) { + if (ctx->dev == NULL) { usleep(250000); continue; } @@ -260,13 +427,34 @@ void *input_dev_thread_func(void *ptr) { const int incoming_events_thread_creation = pthread_create(&incoming_events_thread, NULL, input_read_thread_func, (void*)&ctx); if (incoming_events_thread_creation != 0) { - fprintf(stderr, "Error creating the input thread for device %s: %d\n", libevdev_get_name(ctx.dev), incoming_events_thread_creation); + fprintf(stderr, "Error creating the input thread for device %s: %d\n", libevdev_get_name(ctx->dev), incoming_events_thread_creation); } if (incoming_events_thread_creation == 0) { pthread_join(incoming_events_thread, NULL); } } +} +void *input_dev_thread_func(void *ptr) { + input_dev_t *in_dev = (input_dev_t*)ptr; + + struct input_ctx ctx = { + .dev = NULL, + .queue = in_dev->queue, + }; + + for (int h = 0; h < MAX_MESSAGES_IN_FLIGHT; ++h) { + ctx.messages[h].flags = MESSAGE_FLAGS_HANDLE_DONE; + ctx.messages[h].ev_size = DEFAULT_EVENTS_IN_REPORT; + ctx.messages[h].ev = malloc(sizeof(struct input_event) * ctx.messages[h].ev_size); + } + + if (in_dev->dev_type == input_dev_type_uinput) { + input_udev(in_dev, &ctx); + } else if (in_dev->dev_type == input_dev_type_iio) { + input_iio(in_dev, &ctx); + } + return NULL; } diff --git a/output_dev.c b/output_dev.c index 5f349c9..9fab7fa 100644 --- a/output_dev.c +++ b/output_dev.c @@ -343,6 +343,10 @@ void *output_dev_thread_func(void *ptr) { const int fd = out_dev->fd; + gettimeofday(&now, NULL); + __time_t secAtInit = now.tv_sec; + __time_t usecAtInit = now.tv_usec; + for (;;) { void *raw_ev; const int pop_res = queue_pop_timeout(out_dev->queue, &raw_ev, 1000); @@ -382,7 +386,19 @@ void *output_dev_thread_func(void *ptr) { ); } } - + + gettimeofday(&now, NULL); + const struct input_event timestamp_ev = { + .code = MSC_TIMESTAMP, + .type = EV_MSC, + .value = (now.tv_sec - secAtInit)*1000000 + (now.tv_usec - usecAtInit), + .time = now, + }; + const ssize_t timestamp_written = write(fd, (void*)×tamp_ev, sizeof(timestamp_ev)); + if (timestamp_written != sizeof(timestamp_ev)) { + fprintf(stderr, "Error in sync: written %ld bytes out of %ld\n", timestamp_written, sizeof(timestamp_ev)); + } + gettimeofday(&now, NULL); const struct input_event syn_ev = { .code = SYN_REPORT, diff --git a/rogue_enemy.h b/rogue_enemy.h index 85852df..4d96f72 100644 --- a/rogue_enemy.h +++ b/rogue_enemy.h @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include