NeroReflex/ROGueENEMY
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Generate iio events

Browse source
This commit is contained in:
Denis 2023-11-10 21:08:53 +01:00
parent ef52084de3
commit 574e66474f
No known key found for this signature in database
GPG key ID: DD9B63F805CF5C03
6 changed files with 517 additions and 30 deletions
Download patch file Download diff file Expand all files Collapse all files

4
Makefile
View file

@ -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)

211
dev_iio.c Normal file
View file

@ -0,0 +1,211 @@
#include "dev_iio.h"
#include <stdlib.h>
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;
}

71
dev_iio.h Normal file
View file

@ -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
);

242
input_dev.c
View file

@ -1,6 +1,7 @@
#include "input_dev.h"
#include "message.h"
#include "queue.h"
#include "dev_iio.h"
#include <libevdev-1.0/libevdev/libevdev.h>
#include <linux/input-event-codes.h>
@ -15,6 +16,7 @@
#include <stdio.h>
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;
}

18
output_dev.c
View file

@ -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*)&timestamp_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,

1
rogue_enemy.h
View file

@ -9,6 +9,7 @@
#include <errno.h>
#include <unistd.h>
#include <string.h>
#include <math.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/ipc.h>