From 8a7ab9c8e82262be028abaa7a773503bd6ac90b6 Mon Sep 17 00:00:00 2001 From: Denis Date: Fri, 17 Nov 2023 01:24:05 +0100 Subject: [PATCH] refactor to better support IMU data --- input_dev.c | 88 ++++++++++++++---------------- input_dev.h | 11 ++-- main.c | 10 ++-- message.h | 37 +++++++++++-- output_dev.c | 151 +++++++++++++++++++++++++++------------------------ 5 files changed, 163 insertions(+), 134 deletions(-) diff --git a/input_dev.c b/input_dev.c index c5b778a..9cf0934 100644 --- a/input_dev.c +++ b/input_dev.c @@ -21,9 +21,9 @@ static const char *iio_path = "/sys/bus/iio/devices/"; static uint32_t gyroscope_mouse_translation = 0; -uint32_t input_filter_imu_identity(struct input_event* events, size_t* size, uint32_t* count) { +uint32_t input_filter_imu_identity(struct input_event* events, size_t* size, uint32_t* count, uint32_t* flags) { int32_t gyro_x = 0, gyro_y = 0, gyro_z = 0, accel_x = 0, accel_y = 0, accel_z = 0; - +/* if (gyroscope_mouse_translation > 0) { for (uint32_t i = 0; i < *count; ++i) { if (events[i].type != EV_ABS) { @@ -63,28 +63,31 @@ uint32_t input_filter_imu_identity(struct input_event* events, size_t* size, uin *count = w; - return INPUT_FILTER_FLAGS_PRESERVE_TIME | INPUT_FILTER_FLAGS_MOUSE; + flags |= EV_MESSAGE_FLAGS_PRESERVE_TIME | INPUT_FILTER_FLAGS_MOUSE; } - - //return INPUT_FILTER_FLAGS_PRESERVE_TIME | INPUT_FILTER_FLAGS_IMU; +*/ return INPUT_FILTER_FLAGS_DO_NOT_EMIT; } -uint32_t input_filter_identity(struct input_event* events, size_t* size, uint32_t* count) { +uint32_t input_filter_identity(struct input_event* events, size_t* size, uint32_t* count, uint32_t* flags) { return INPUT_FILTER_FLAGS_NONE; } -uint32_t input_filter_asus_kb(struct input_event* events, size_t* size, uint32_t* count) { +uint32_t input_filter_asus_kb(struct input_event* events, size_t* size, uint32_t* count, uint32_t* flags) { static int F15_status = 0; if (events[0].type == EV_REL) { - return INPUT_FILTER_FLAGS_MOUSE; + *flags |= EV_MESSAGE_FLAGS_MOUSE; + + return 0; } else if ((events[0].type == EV_KEY) || (events[1].type == EV_KEY)) { if ((events[0].code == BTN_MIDDLE) || (events[0].code == BTN_LEFT) || (events[0].code == BTN_RIGHT)) { - return INPUT_FILTER_FLAGS_PRESERVE_TIME | INPUT_FILTER_FLAGS_MOUSE; + *flags |= EV_MESSAGE_FLAGS_PRESERVE_TIME | EV_MESSAGE_FLAGS_MOUSE; } else if ((events[1].code == BTN_MIDDLE) || (events[1].code == BTN_LEFT) || (events[1].code == BTN_RIGHT)) { - return INPUT_FILTER_FLAGS_PRESERVE_TIME | INPUT_FILTER_FLAGS_MOUSE; + *flags |= EV_MESSAGE_FLAGS_PRESERVE_TIME | EV_MESSAGE_FLAGS_MOUSE; } + + return 0; } if ((*count >= 2) && (events[0].type == EV_MSC) && (events[0].code == MSC_SCAN)) { @@ -265,7 +268,7 @@ struct input_ctx { dev_iio_t *iio_dev; queue_t* queue; message_t messages[MAX_MESSAGES_IN_FLIGHT]; - input_filter_t input_filter_fn; + ev_input_filter_t input_filter_fn; }; static void* iio_read_thread_func(void* ptr) { @@ -280,7 +283,7 @@ static void* iio_read_thread_func(void* ptr) { 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; + //TODO: msg->ev_count = 0; break; } } @@ -291,6 +294,7 @@ static void* iio_read_thread_func(void* ptr) { continue; } + /* rc = dev_iio_read(ctx->iio_dev, msg->ev, msg->ev_size, &msg->ev_count); if (rc == 0) { // OK: good read. go on.... @@ -307,18 +311,6 @@ static void* iio_read_thread_func(void* ptr) { const uint32_t input_filter_res = ctx->input_filter_fn(msg->ev, &msg->ev_size, &msg->ev_count); - if ((input_filter_res & INPUT_FILTER_FLAGS_IMU) != 0) { - msg->flags |= INPUT_FILTER_FLAGS_IMU; - } - - if ((input_filter_res & INPUT_FILTER_FLAGS_MOUSE) != 0) { - msg->flags |= INPUT_FILTER_FLAGS_MOUSE; - } - - if ((input_filter_res & INPUT_FILTER_FLAGS_PRESERVE_TIME) != 0) { - msg->flags |= INPUT_FILTER_FLAGS_PRESERVE_TIME; - } - if (((input_filter_res & INPUT_FILTER_FLAGS_DO_NOT_EMIT) == 0) && (msg->ev_count > 0)) { if (queue_push(ctx->queue, (void*)msg) != 0) { fprintf(stderr, "Error pushing iio event.\n"); @@ -330,6 +322,7 @@ static void* iio_read_thread_func(void* ptr) { // flag the memory to be safe to reuse msg->flags |= MESSAGE_FLAGS_HANDLE_DONE; } + */ usleep(100); @@ -355,7 +348,7 @@ static void* input_read_thread_func(void* ptr) { 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; + msg->data.event.ev_count = 0; break; } } @@ -383,7 +376,7 @@ static void* input_read_thread_func(void* ptr) { } if ((!has_syn) || ((has_syn) && (!is_syn))) { - if ((msg->ev_count+1) == msg->ev_size) { + if ((msg->data.event.ev_count+1) == msg->data.event.ev_size) { // TODO: perform a memove fprintf(stderr, "MEMMOVE NEEDED\n"); } else { @@ -397,8 +390,8 @@ static void* input_read_thread_func(void* ptr) { #endif // just copy the input event - msg->ev[msg->ev_count] = read_ev; - ++msg->ev_count; + msg->data.event.ev[msg->data.event.ev_count] = read_ev; + ++msg->data.event.ev_count; } } @@ -409,22 +402,11 @@ static void* input_read_thread_func(void* ptr) { // clear out flags msg->flags = 0x00000000U; + msg->data.event.ev_flags = 0x00000000U; - const uint32_t input_filter_res = ctx->input_filter_fn(msg->ev, &msg->ev_size, &msg->ev_count); + const uint32_t input_filter_res = ctx->input_filter_fn(msg->data.event.ev, &msg->data.event.ev_size, &msg->data.event.ev_count, &msg->data.event.ev_flags); - if ((input_filter_res & INPUT_FILTER_FLAGS_IMU) != 0) { - msg->flags |= INPUT_FILTER_FLAGS_IMU; - } - - if ((input_filter_res & INPUT_FILTER_FLAGS_MOUSE) != 0) { - msg->flags |= INPUT_FILTER_FLAGS_MOUSE; - } - - if ((input_filter_res & INPUT_FILTER_FLAGS_PRESERVE_TIME) != 0) { - msg->flags |= INPUT_FILTER_FLAGS_PRESERVE_TIME; - } - - if (((input_filter_res & INPUT_FILTER_FLAGS_DO_NOT_EMIT) == 0) && (msg->ev_count > 0)) { + if (((input_filter_res & INPUT_FILTER_FLAGS_DO_NOT_EMIT) == 0) && (msg->data.event.ev_count > 0)) { if (queue_push(ctx->queue, (void*)msg) != 0) { fprintf(stderr, "Error pushing event.\n"); @@ -674,18 +656,28 @@ void *input_dev_thread_func(void *ptr) { struct input_ctx ctx = { .dev = NULL, .queue = in_dev->queue, - .input_filter_fn = in_dev->input_filter_fn, + .input_filter_fn = in_dev->ev_input_filter_fn, }; - 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) { + // prepare space and empty messages + for (int h = 0; h < MAX_MESSAGES_IN_FLIGHT; ++h) { + ctx.messages[h].flags = MESSAGE_FLAGS_HANDLE_DONE; + ctx.messages[h].type = MSG_TYPE_EV; + ctx.messages[h].data.event.ev_size = DEFAULT_EVENTS_IN_REPORT; + ctx.messages[h].data.event.ev = malloc(sizeof(struct input_event) * ctx.messages[h].data.event.ev_size); + } + input_udev(in_dev, &ctx); } else if (in_dev->dev_type == input_dev_type_iio) { + // prepare space and empty messages + for (int h = 0; h < MAX_MESSAGES_IN_FLIGHT; ++h) { + ctx.messages[h].flags = MESSAGE_FLAGS_HANDLE_DONE; + ctx.messages[h].type = MSG_TYPE_IMU; + } + input_iio(in_dev, &ctx); } diff --git a/input_dev.h b/input_dev.h index 199cade..4693dc4 100644 --- a/input_dev.h +++ b/input_dev.h @@ -8,6 +8,8 @@ #define INPUT_DEV_CTRL_FLAG_EXIT 0x00000001U +typedef uint32_t (*ev_input_filter_t)(struct input_event*, size_t*, uint32_t*, uint32_t*); + typedef enum input_dev_type { input_dev_type_uinput, input_dev_type_iio, @@ -31,15 +33,16 @@ typedef struct input_dev { queue_t *queue; - input_filter_t input_filter_fn; + ev_input_filter_t ev_input_filter_fn; + } input_dev_t; void *input_dev_thread_func(void *ptr); int open_and_hide_input(); -uint32_t input_filter_imu_identity(struct input_event* events, size_t* size, uint32_t* count); +uint32_t input_filter_imu_identity(struct input_event* events, size_t* size, uint32_t* count, uint32_t* flags); -uint32_t input_filter_identity(struct input_event* events, size_t* size, uint32_t* count); +uint32_t input_filter_identity(struct input_event* events, size_t* size, uint32_t* count, uint32_t* flags); -uint32_t input_filter_asus_kb(struct input_event*, size_t*, uint32_t*); \ No newline at end of file +uint32_t input_filter_asus_kb(struct input_event*, size_t*, uint32_t*, uint32_t* flags); \ No newline at end of file diff --git a/main.c b/main.c index 81677e6..d14ae9c 100644 --- a/main.c +++ b/main.c @@ -22,7 +22,7 @@ static input_dev_t in_iio_dev = { .crtl_flags = 0x00000000U, .iio_filters = &in_iio_filters, .queue = &global_ev_queue, - .input_filter_fn = input_filter_imu_identity, + //.input_filter_fn = input_filter_imu_identity, }; static uinput_filters_t in_asus_kb_1_filters = { @@ -34,7 +34,7 @@ static input_dev_t in_asus_kb_1_dev = { .crtl_flags = 0x00000000U, .ev_filters = &in_asus_kb_1_filters, .queue = &global_ev_queue, - .input_filter_fn = input_filter_asus_kb, + .ev_input_filter_fn = input_filter_asus_kb, }; static uinput_filters_t in_asus_kb_2_filters = { @@ -46,7 +46,7 @@ static input_dev_t in_asus_kb_2_dev = { .crtl_flags = 0x00000000U, .ev_filters = &in_asus_kb_2_filters, .queue = &global_ev_queue, - .input_filter_fn = input_filter_asus_kb, + .ev_input_filter_fn = input_filter_asus_kb, }; static uinput_filters_t in_asus_kb_3_filters = { @@ -58,7 +58,7 @@ static input_dev_t in_asus_kb_3_dev = { .crtl_flags = 0x00000000U, .ev_filters = &in_asus_kb_3_filters, .queue = &global_ev_queue, - .input_filter_fn = input_filter_asus_kb, + .ev_input_filter_fn = input_filter_asus_kb, }; static uinput_filters_t in_xbox_filters = { @@ -70,7 +70,7 @@ static input_dev_t in_xbox_dev = { .crtl_flags = 0x00000000U, .ev_filters = &in_xbox_filters, .queue = &global_ev_queue, - .input_filter_fn = input_filter_identity, + .ev_input_filter_fn = input_filter_identity, }; void request_termination() { diff --git a/message.h b/message.h index fbfa715..879a75a 100644 --- a/message.h +++ b/message.h @@ -4,20 +4,45 @@ #define MESSAGE_FLAGS_HANDLE_DONE 0x00000001U -typedef struct message { +typedef struct ev_message { struct input_event* ev; + uint32_t ev_flags; + uint32_t ev_count; size_t ev_size; +} ev_message_t; + +typedef struct imu_message { + int32_t gyro_x; + int32_t gyro_y; + int32_t gyro_z; + + int32_t accel_x; + int32_t accel_y; + int32_t accel_z; +} imu_message_t; + +typedef enum message_type { + MSG_TYPE_EV = 0, + MSG_TYPE_IMU, +} message_type_t; + +typedef struct message { + message_type_t type; + + union { + imu_message_t imu; + ev_message_t event; + } data; + volatile uint32_t flags; } message_t; #define INPUT_FILTER_FLAGS_NONE 0x00000000U #define INPUT_FILTER_FLAGS_DO_NOT_EMIT 0x00000001U -#define INPUT_FILTER_FLAGS_PRESERVE_TIME 0x00000002U -#define INPUT_FILTER_FLAGS_IMU 0x00000004U -#define INPUT_FILTER_FLAGS_MOUSE 0x00000008U - -typedef uint32_t (*input_filter_t)(struct input_event*, size_t*, uint32_t*); \ No newline at end of file +#define EV_MESSAGE_FLAGS_PRESERVE_TIME 0x00000002U +#define EV_MESSAGE_FLAGS_IMU 0x00000004U +#define EV_MESSAGE_FLAGS_MOUSE 0x00000008U diff --git a/output_dev.c b/output_dev.c index 5ad4f3a..a21727b 100644 --- a/output_dev.c +++ b/output_dev.c @@ -453,6 +453,85 @@ create_output_dev_err: return fd; } +static void handle_ev(output_dev_t *out_dev, const message_t* msg) { + struct timeval now = {0}; + + int fd = out_dev->gamepad_fd; + if ((msg->flags & EV_MESSAGE_FLAGS_IMU) != 0) { + fd = out_dev->imu_fd; + } else if ((msg->flags & EV_MESSAGE_FLAGS_MOUSE) != 0) { + fd = out_dev->mouse_fd; + } else { + fd = out_dev->gamepad_fd; + } + + for (uint32_t i = 0; i < msg->data.event.ev_count; ++i) { + struct input_event ev = { + .code = msg->data.event.ev[i].code, + .type = msg->data.event.ev[i].type, + .value = msg->data.event.ev[i].value, + .time = msg->data.event.ev[i].time, + }; + + if ((msg->data.event.ev_flags & EV_MESSAGE_FLAGS_PRESERVE_TIME) == 0) { + gettimeofday(&now, NULL); + ev.time = now; + } + +#if defined(INCLUDE_OUTPUT_DEBUG) + printf( + "Output: Received event %s (%s): %d\n", + libevdev_event_type_get_name(ev.type), + libevdev_event_code_get_name(ev.type, ev.code), + ev.value + ); +#endif + + const ssize_t written = write(fd, (void*)&ev, sizeof(ev)); + if (written != sizeof(ev)) { + fprintf( + stderr, + "Error writing event %s %s %d: written %ld bytes out of %ld\n", + libevdev_event_type_get_name(ev.type), + libevdev_event_code_get_name(ev.type, ev.code), + ev.value, + written, + sizeof(ev) + ); + } + } + +#if defined(INCLUDE_TIMESTAMP) + 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)); + } +#endif + + gettimeofday(&now, NULL); + const struct input_event syn_ev = { + .code = SYN_REPORT, + .type = EV_SYN, + .value = 0, + .time = now, + }; + const ssize_t sync_written = write(fd, (void*)&syn_ev, sizeof(syn_ev)); + if (sync_written != sizeof(syn_ev)) { + fprintf(stderr, "Error in sync: written %ld bytes out of %ld\n", sync_written, sizeof(syn_ev)); + } +} + +static void handle_msg(output_dev_t *out_dev, const message_t* msg) { + +} + void *output_dev_thread_func(void *ptr) { output_dev_t *out_dev = (output_dev_t*)ptr; struct timeval now = {0}; @@ -468,77 +547,7 @@ void *output_dev_thread_func(void *ptr) { const int pop_res = queue_pop_timeout(out_dev->queue, &raw_ev, 1000); if (pop_res == 0) { message_t *const msg = (message_t*)raw_ev; - - int fd = out_dev->gamepad_fd; - if ((msg->flags & INPUT_FILTER_FLAGS_IMU) != 0) { - fd = out_dev->imu_fd; - } else if ((msg->flags & INPUT_FILTER_FLAGS_MOUSE) != 0) { - fd = out_dev->mouse_fd; - } else { - fd = out_dev->gamepad_fd; - } - - for (uint32_t i = 0; i < msg->ev_count; ++i) { - struct input_event ev = { - .code = msg->ev[i].code, - .type = msg->ev[i].type, - .value = msg->ev[i].value, - .time = msg->ev[i].time, - }; - - if ((msg->flags & INPUT_FILTER_FLAGS_PRESERVE_TIME) == 0) { - gettimeofday(&now, NULL); - ev.time = now; - } - -#if defined(INCLUDE_OUTPUT_DEBUG) - printf( - "Output: Received event %s (%s): %d\n", - libevdev_event_type_get_name(ev.type), - libevdev_event_code_get_name(ev.type, ev.code), - ev.value - ); -#endif - - const ssize_t written = write(fd, (void*)&ev, sizeof(ev)); - if (written != sizeof(ev)) { - fprintf( - stderr, - "Error writing event %s %s %d: written %ld bytes out of %ld\n", - libevdev_event_type_get_name(ev.type), - libevdev_event_code_get_name(ev.type, ev.code), - ev.value, - written, - sizeof(ev) - ); - } - } - -#if defined(INCLUDE_TIMESTAMP) - 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)); - } -#endif - - gettimeofday(&now, NULL); - const struct input_event syn_ev = { - .code = SYN_REPORT, - .type = EV_SYN, - .value = 0, - .time = now, - }; - const ssize_t sync_written = write(fd, (void*)&syn_ev, sizeof(syn_ev)); - if (sync_written != sizeof(syn_ev)) { - fprintf(stderr, "Error in sync: written %ld bytes out of %ld\n", sync_written, sizeof(syn_ev)); - } + handle_msg(out_dev, msg); // from now on it's forbidden to use this memory msg->flags |= MESSAGE_FLAGS_HANDLE_DONE;