diff --git a/dev_iio.c b/dev_iio.c index e945953..7997a9a 100644 --- a/dev_iio.c +++ b/dev_iio.c @@ -1,14 +1,62 @@ #include "dev_iio.h" #include +static char* read_file(const char* base_path, const char *file) { + char* fdir = NULL; + long len = 0; + + len = strlen(base_path) + strlen(file) + 1; + fdir = malloc(len); + if (fdir == NULL) { + fprintf(stderr, "Cannot allocate %ld bytes for device path, device skipped.\n", len); + goto read_file_err; + } + strcpy(fdir, base_path); + strcat(fdir, file); + + if (access(fdir, F_OK) == 0) { + FILE* fp = fopen(fdir, "r"); + if (fp != NULL) { + fseek(fp, 0L, SEEK_END); + len = ftell(fp); + rewind(fp); + + free(fdir); + fdir = NULL; + + len += 1; + char* res = malloc(len); + if (res != NULL) { + fread(res, 1, len, fp); + fclose(fp); + + return res; + } else { + fprintf(stderr, "Cannot allocate %ld bytes for %s content.\n", len, fdir); + goto read_file_err; + } + } else { + fprintf(stderr, "Cannot open file %s.\n", fdir); + free(fdir); + goto read_file_err; + } + } else { + free(fdir); + goto read_file_err; + } + +read_file_err: + return NULL; +} + 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; + dev_iio_t *iio = malloc(sizeof(dev_iio_t)); + iio->anglvel_x_fd = NULL; + iio->anglvel_y_fd = NULL; + iio->anglvel_z_fd = NULL; + iio->accel_x_fd = NULL; + iio->accel_y_fd = NULL; + iio->accel_z_fd = NULL; iio->accel_scale_x = 0.0f; iio->accel_scale_y = 0.0f; @@ -24,18 +72,106 @@ dev_iio_t* dev_iio_create(const char* path) { iio->outer_anglvel_scale_y = GYRO_SCALE; iio->outer_anglvel_scale_z = GYRO_SCALE; + const long path_len = strlen(path) + 1; + iio->path = malloc(path_len); + if (iio->path == NULL) { + fprintf(stderr, "Cannot allocate %ld bytes for device name, device skipped.\n", path_len); + free(iio); + iio = NULL; + goto dev_iio_create_err; + } + strcpy(iio->path, path); + + // ============================================= DEVICE NAME ================================================ + iio->name = read_file(iio->path, "/name"); + if (iio->name == NULL) { + free(iio); + iio = NULL; + goto dev_iio_create_err; + } + // ========================================================================================================== + + // ========================================== in_anglvel_scale ============================================== + { + char* const anglvel_scale = read_file(iio->path, "/in_anglvel_scale"); + if (anglvel_scale != NULL) { + iio->anglvel_scale_x = iio->anglvel_scale_y = iio->anglvel_scale_z = strtod(anglvel_scale, NULL); + free((void*)anglvel_scale); + } else { + // TODO: what about if those are plit in in_anglvel_{x,y,z}_scale? + + free(iio); + iio = NULL; + goto dev_iio_create_err; + } + } + // ========================================================================================================== + + // =========================================== in_accel_scale =============================================== + { + char* const accel_scale = read_file(iio->path, "/in_accel_scale"); + if (accel_scale != NULL) { + iio->accel_scale_x = iio->accel_scale_y = iio->accel_scale_z = strtod(accel_scale, NULL); + free((void*)accel_scale); + } else { + // TODO: what about if those are plit in in_accel_{x,y,z}_scale? + + free(iio); + iio = NULL; + goto dev_iio_create_err; + } + } + // ========================================================================================================== + + const size_t tmp_sz = path_len + 128 + 1; + char* const tmp = malloc(tmp_sz); + + memset(tmp, 0, tmp_sz); + strcat(tmp, iio->path); + strcat(tmp, "/in_accel_x_raw"); + iio->accel_x_fd = fopen(tmp, "r"); + + memset(tmp, 0, tmp_sz); + strcat(tmp, iio->path); + strcat(tmp, "/in_accel_y_raw"); + iio->accel_y_fd = fopen(tmp, "r"); + + memset(tmp, 0, tmp_sz); + strcat(tmp, iio->path); + strcat(tmp, "/in_accel_z_raw"); + iio->accel_z_fd = fopen(tmp, "r"); + + memset(tmp, 0, tmp_sz); + strcat(tmp, iio->path); + strcat(tmp, "/in_anglvel_x_raw"); + iio->anglvel_x_fd = fopen(tmp, "r"); + + memset(tmp, 0, tmp_sz); + strcat(tmp, iio->path); + strcat(tmp, "/in_anglvel_y_raw"); + iio->anglvel_y_fd = fopen(tmp, "r"); + + memset(tmp, 0, tmp_sz); + strcat(tmp, iio->path); + strcat(tmp, "/in_anglvel_z_raw"); + iio->anglvel_z_fd = fopen(tmp, "r"); + + free(tmp); + +dev_iio_create_err: return iio; } void dev_iio_destroy(dev_iio_t* iio) { + fclose(iio->accel_x_fd); + fclose(iio->accel_y_fd); + fclose(iio->accel_z_fd); + fclose(iio->anglvel_x_fd); + fclose(iio->anglvel_y_fd); + fclose(iio->anglvel_z_fd); 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); + free(iio); } const char* dev_iio_get_name(const dev_iio_t* iio) { @@ -59,9 +195,10 @@ int dev_iio_read( if (*buf_out == buf_sz) { return -ENOMEM; - } else if (iio->accel_x != -1) { + } else if (iio->accel_x_fd != NULL) { + rewind(iio->accel_x_fd); memset((void*)&tmp[0], 0, sizeof(tmp)); - int tmp_read = read(iio->accel_x, (void*)&tmp[0], sizeof(tmp)); + const int tmp_read = fread((void*)&tmp[0], 1, sizeof(tmp), iio->accel_x_fd); if (tmp_read >= 0) { gettimeofday(&now, NULL); const double val = strtod(&tmp[0], NULL); @@ -73,8 +210,6 @@ int dev_iio_read( 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; } @@ -84,9 +219,10 @@ int dev_iio_read( if (*buf_out == buf_sz) { return -ENOMEM; - } else if (iio->accel_y != -1) { + } else if (iio->accel_y_fd != NULL) { + rewind(iio->accel_y_fd); memset((void*)&tmp[0], 0, sizeof(tmp)); - int tmp_read = read(iio->accel_y, (void*)&tmp[0], sizeof(tmp)); + const int tmp_read = fread((void*)&tmp[0], 1, sizeof(tmp), iio->accel_y_fd); if (tmp_read >= 0) { gettimeofday(&now, NULL); const double val = strtod(&tmp[0], NULL); @@ -98,8 +234,6 @@ int dev_iio_read( 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; } @@ -109,9 +243,10 @@ int dev_iio_read( if (*buf_out == buf_sz) { return -ENOMEM; - } else if (iio->accel_z != -1) { + } else if (iio->accel_z_fd != NULL) { + rewind(iio->accel_z_fd); memset((void*)&tmp[0], 0, sizeof(tmp)); - int tmp_read = read(iio->accel_z, (void*)&tmp[0], sizeof(tmp)); + const int tmp_read = fread((void*)&tmp[0], 1, sizeof(tmp), iio->accel_z_fd); if (tmp_read >= 0) { gettimeofday(&now, NULL); const double val = strtod(&tmp[0], NULL); @@ -123,8 +258,6 @@ int dev_iio_read( 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; } @@ -134,9 +267,10 @@ int dev_iio_read( if (*buf_out == buf_sz) { return -ENOMEM; - } else if (iio->anglvel_x != -1) { + } else if (iio->anglvel_x_fd != NULL) { + rewind(iio->anglvel_x_fd); memset((void*)&tmp[0], 0, sizeof(tmp)); - int tmp_read = read(iio->anglvel_x, (void*)&tmp[0], sizeof(tmp)); + const int tmp_read = fread((void*)&tmp[0], 1, sizeof(tmp), iio->anglvel_x_fd); if (tmp_read >= 0) { gettimeofday(&now, NULL); const double val = strtod(&tmp[0], NULL); @@ -148,8 +282,6 @@ int dev_iio_read( 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; } @@ -159,9 +291,10 @@ int dev_iio_read( if (*buf_out == buf_sz) { return -ENOMEM; - } else if (iio->anglvel_y != -1) { + } else if (iio->anglvel_y_fd != NULL) { + rewind(iio->anglvel_y_fd); memset((void*)&tmp[0], 0, sizeof(tmp)); - int tmp_read = read(iio->anglvel_y, (void*)&tmp[0], sizeof(tmp)); + const int tmp_read = fread((void*)&tmp[0], 1, sizeof(tmp), iio->anglvel_y_fd); if (tmp_read >= 0) { gettimeofday(&now, NULL); const double val = strtod(&tmp[0], NULL); @@ -173,8 +306,6 @@ int dev_iio_read( 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; } @@ -184,9 +315,10 @@ int dev_iio_read( if (*buf_out == buf_sz) { return -ENOMEM; - } else if (iio->anglvel_z != -1) { + } else if (iio->anglvel_z_fd != NULL) { + rewind(iio->anglvel_z_fd); memset((void*)&tmp[0], 0, sizeof(tmp)); - int tmp_read = read(iio->anglvel_z, (void*)&tmp[0], sizeof(tmp)); + const int tmp_read = fread((void*)&tmp[0], 1, sizeof(tmp), iio->anglvel_z_fd); if (tmp_read >= 0) { gettimeofday(&now, NULL); const double val = strtod(&tmp[0], NULL); @@ -198,8 +330,6 @@ int dev_iio_read( 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; } diff --git a/dev_iio.h b/dev_iio.h index 35e6544..9784dac 100644 --- a/dev_iio.h +++ b/dev_iio.h @@ -13,29 +13,21 @@ typedef struct dev_iio { 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; + FILE* accel_x_fd; + FILE* accel_y_fd; + FILE* accel_z_fd; 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; + FILE* anglvel_x_fd; + FILE* anglvel_y_fd; + FILE* anglvel_z_fd; double anglvel_scale_x; double anglvel_scale_y; diff --git a/input_dev.c b/input_dev.c index f4a2f44..ee0c7d3 100644 --- a/input_dev.c +++ b/input_dev.c @@ -128,6 +128,8 @@ static void* iio_read_thread_func(void* ptr) { msg->flags |= MESSAGE_FLAGS_HANDLE_DONE; } + usleep(100); + // either way.... fill a new buffer on the next cycle msg = NULL; } while (rc == 1 || rc == 0 || rc == -EAGAIN); diff --git a/main.c b/main.c index 12502c5..4a0cd37 100644 --- a/main.c +++ b/main.c @@ -18,6 +18,17 @@ static output_dev_t out_gamepadd_dev = { .queue = &gamepad_ev, }; +static iio_filters_t in_iio_filters = { + .name = "bmi323", +}; + +static input_dev_t in_iio_dev = { + .dev_type = input_dev_type_iio, + .crtl_flags = 0x00000000U, + .iio_filters = &in_iio_filters, + .queue = &imu_ev, +}; + static uinput_filters_t in_asus_kb_1_filters = { .name = "Asus Keyboard", }; @@ -112,7 +123,7 @@ int main(int argc, char ** argv) { int ret = 0; pthread_t imu_thread, gamepad_thread; - pthread_t xbox_thread, asus_kb_1_thread, asus_kb_2_thread, asus_kb_3_thread; + pthread_t xbox_thread, asus_kb_1_thread, asus_kb_2_thread, asus_kb_3_thread, iio_thread; const int imu_thread_creation = pthread_create(&imu_thread, NULL, output_dev_thread_func, (void*)(&out_imu_dev)); if (imu_thread_creation != 0) { @@ -162,6 +173,17 @@ int main(int argc, char ** argv) { goto asus_kb_3_thread_err; } + const int iio_thread_creation = pthread_create(&iio_thread, NULL, input_dev_thread_func, (void*)(&in_iio_dev)); + if (iio_thread_creation != 0) { + fprintf(stderr, "Error creating iio input thread: %d\n", asus_kb_3_thread_creation); + ret = -1; + request_termination(); + goto iio_thread_err; + } + + pthread_join(iio_thread, NULL); + +iio_thread_err: pthread_join(asus_kb_3_thread, NULL); asus_kb_3_thread_err: