418 lines
12 KiB
C
418 lines
12 KiB
C
#include "dev_iio.h"
|
|
#include <stdlib.h>
|
|
|
|
#define MAX_PATH_LEN 512
|
|
|
|
struct read_file_res {
|
|
char* buf;
|
|
unsigned long len;
|
|
};
|
|
|
|
static struct read_file_res read_file(const char* base_path, const char *file) {
|
|
struct read_file_res res = {
|
|
.buf = NULL,
|
|
.len = 0,
|
|
};
|
|
|
|
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);
|
|
|
|
len += 1;
|
|
res.buf = malloc(len);
|
|
if (res.buf != NULL) {
|
|
res.len = fread(res.buf, 1, len, fp);
|
|
//printf("Read %lu bytes from file %s\n", read_bytes, fdir);
|
|
} else {
|
|
fprintf(stderr, "Cannot allocate %ld bytes for %s content.\n", len, fdir);
|
|
}
|
|
|
|
fclose(fp);
|
|
} else {
|
|
fprintf(stderr, "Cannot open file %s.\n", fdir);
|
|
}
|
|
} else {
|
|
fprintf(stderr, "File %s does not exists.\n", fdir);
|
|
}
|
|
|
|
free(fdir);
|
|
fdir = NULL;
|
|
|
|
read_file_err:
|
|
return res;
|
|
}
|
|
|
|
int write_file(const char* base_path, const char *file, const void* buf, size_t buf_sz) {
|
|
char* fdir = NULL;
|
|
|
|
int res = 0;
|
|
|
|
const size_t 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 write_file_err;
|
|
}
|
|
strcpy(fdir, base_path);
|
|
strcat(fdir, file);
|
|
|
|
if (access(fdir, F_OK) == 0) {
|
|
FILE* fp = fopen(fdir, "w");
|
|
if (fp != NULL) {
|
|
res = fwrite(buf, 1, buf_sz, fp);
|
|
if (res >= buf_sz) {
|
|
printf("Written %d bytes to file %s\n", res, fdir);
|
|
} else {
|
|
fprintf(stderr, "Cannot write to %s: %d.\n", fdir, res);
|
|
}
|
|
|
|
fclose(fp);
|
|
} else {
|
|
fprintf(stderr, "Cannot open file %s.\n", fdir);
|
|
}
|
|
} else {
|
|
fprintf(stderr, "File %s does not exists.\n", fdir);
|
|
}
|
|
|
|
free(fdir);
|
|
fdir = NULL;
|
|
|
|
write_file_err:
|
|
return res;
|
|
}
|
|
|
|
static int dev_iio_create(char* dev_path, const char* path, dev_iio_t **const out_iio) {
|
|
int res = -ENOENT;
|
|
|
|
*out_iio = malloc(sizeof(dev_iio_t));
|
|
if (*out_iio == NULL) {
|
|
fprintf(stderr, "Cannot allocate memory for iio device\n");
|
|
res = -ENOMEM;
|
|
goto dev_iio_create_err;
|
|
}
|
|
|
|
(*out_iio)->flags = 0x00000000U;
|
|
(*out_iio)->fd = -1;
|
|
|
|
(*out_iio)->accel_scale_x = 0.0f;
|
|
(*out_iio)->accel_scale_y = 0.0f;
|
|
(*out_iio)->accel_scale_z = 0.0f;
|
|
(*out_iio)->anglvel_scale_x = 0.0f;
|
|
(*out_iio)->anglvel_scale_y = 0.0f;
|
|
(*out_iio)->anglvel_scale_z = 0.0f;
|
|
(*out_iio)->temp_scale = 0.0f;
|
|
|
|
(*out_iio)->outer_accel_scale_x = ACCEL_SCALE;
|
|
(*out_iio)->outer_accel_scale_y = ACCEL_SCALE;
|
|
(*out_iio)->outer_accel_scale_z = ACCEL_SCALE;
|
|
(*out_iio)->outer_anglvel_scale_x = GYRO_SCALE;
|
|
(*out_iio)->outer_anglvel_scale_y = GYRO_SCALE;
|
|
(*out_iio)->outer_anglvel_scale_z = GYRO_SCALE;
|
|
(*out_iio)->outer_temp_scale = 0.0;
|
|
|
|
const long path_len = strlen(path) + 1;
|
|
(*out_iio)->path = malloc(path_len);
|
|
if ((*out_iio)->path == NULL) {
|
|
fprintf(stderr, "Cannot allocate %ld bytes for device name, device skipped.\n", path_len);
|
|
goto dev_iio_create_err;
|
|
}
|
|
strcpy((*out_iio)->path, path);
|
|
|
|
const long dev_path_len = strlen(dev_path) + 1;
|
|
(*out_iio)->dev_path = malloc(dev_path_len);
|
|
if ((*out_iio)->dev_path == NULL) {
|
|
fprintf(stderr, "Cannot allocate %ld bytes for device /dev path, device skipped.\n", path_len);
|
|
goto dev_iio_create_err;
|
|
}
|
|
strcpy((*out_iio)->dev_path, dev_path);
|
|
|
|
(*out_iio)->fd = open((*out_iio)->dev_path, O_RDONLY);
|
|
if ((*out_iio)->fd < 0) {
|
|
fprintf(stderr, "Error opening %s: %d\n", (*out_iio)->dev_path, errno);
|
|
res = errno;
|
|
res = res < 0 ? res : -1 * res;
|
|
goto dev_iio_create_err;
|
|
}
|
|
|
|
struct read_file_res rr;
|
|
|
|
// ============================================= DEVICE NAME ================================================
|
|
rr = read_file((*out_iio)->path, "/name");
|
|
(*out_iio)->name = rr.buf;
|
|
if ((*out_iio)->name == NULL) {
|
|
fprintf(stderr, "Unable to read iio device name.\n");
|
|
goto dev_iio_create_err;
|
|
} else {
|
|
int idx = strlen((*out_iio)->name) - 1;
|
|
if (((*out_iio)->name[idx] == '\n') || (((*out_iio)->name[idx] == '\t'))) {
|
|
(*out_iio)->name[idx] = '\0';
|
|
}
|
|
}
|
|
// ==========================================================================================================
|
|
|
|
// ========================================== in_anglvel_scale ==============================================
|
|
{
|
|
const char *scale_main_file = "/in_anglvel_scale";
|
|
rr = read_file((*out_iio)->path, scale_main_file);
|
|
char* const anglvel_scale = rr.buf;
|
|
if (anglvel_scale != NULL) {
|
|
(*out_iio)->flags |= DEV_IIO_HAS_ANGLVEL;
|
|
(*out_iio)->anglvel_scale_x = (*out_iio)->anglvel_scale_y = (*out_iio)->anglvel_scale_z = strtod(anglvel_scale, NULL);
|
|
free((void*)anglvel_scale);
|
|
} else {
|
|
// TODO: what about if those are split in in_anglvel_{x,y,z}_scale?
|
|
fprintf(stderr, "Unable to read in_anglvel_scale from path %s%s.\n", (*out_iio)->path, scale_main_file);
|
|
goto dev_iio_create_err;
|
|
}
|
|
}
|
|
// ==========================================================================================================
|
|
|
|
// =========================================== in_accel_scale ===============================================
|
|
{
|
|
const char *scale_main_file = "/in_accel_scale";
|
|
rr = read_file((*out_iio)->path, scale_main_file);
|
|
char* const accel_scale = rr.buf;
|
|
if (accel_scale != NULL) {
|
|
(*out_iio)->flags |= DEV_IIO_HAS_ACCEL;
|
|
(*out_iio)->accel_scale_x = (*out_iio)->accel_scale_y = (*out_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?
|
|
fprintf(stderr, "Unable to read in_accel_scale file from path %s%s.\n", (*out_iio)->path, scale_main_file);
|
|
goto dev_iio_create_err;
|
|
}
|
|
}
|
|
// ==========================================================================================================
|
|
|
|
// ============================================= temp_scale =================================================
|
|
{
|
|
const char *scale_main_file = "/in_temp_scale";
|
|
|
|
rr = read_file((*out_iio)->path, scale_main_file);
|
|
char* const accel_scale = rr.buf;
|
|
if (accel_scale != NULL) {
|
|
(*out_iio)->temp_scale = strtod(accel_scale, NULL);
|
|
free((void*)accel_scale);
|
|
} else {
|
|
fprintf(stderr, "Unable to read in_temp_scale file from path %s%s.\n", (*out_iio)->path, scale_main_file);
|
|
goto dev_iio_create_err;
|
|
}
|
|
}
|
|
// ==========================================================================================================
|
|
|
|
res = 0;
|
|
|
|
dev_iio_create_err:
|
|
if (res != 0) {
|
|
if ((*out_iio)->fd > 0) {
|
|
close((*out_iio)->fd);
|
|
}
|
|
|
|
if ((*out_iio)->path != NULL) {
|
|
free((*out_iio)->path);
|
|
(*out_iio)->path = NULL;
|
|
}
|
|
|
|
if (*out_iio == NULL) {
|
|
free(*out_iio);
|
|
*out_iio = NULL;
|
|
}
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
int dev_iio_change_anglvel_sampling_freq(const dev_iio_t *const iio, const char *const freq_str_hz) {
|
|
int res = -EINVAL;
|
|
if (!dev_iio_has_anglvel(iio)) {
|
|
res = -ENOENT;
|
|
goto dev_iio_change_anglvel_sampling_freq_err;
|
|
}
|
|
|
|
res = write_file(iio->path, "/in_anglvel_sampling_frequency", freq_str_hz, strlen(freq_str_hz));
|
|
|
|
dev_iio_change_anglvel_sampling_freq_err:
|
|
return res;
|
|
}
|
|
|
|
int dev_iio_change_accel_sampling_freq(const dev_iio_t *const iio, const char *const freq_str_hz) {
|
|
int res = -EINVAL;
|
|
if (!dev_iio_has_anglvel(iio)) {
|
|
res = -ENOENT;
|
|
goto dev_iio_change_accel_sampling_freq_err;
|
|
}
|
|
|
|
res = write_file(iio->path, "/in_accel_sampling_frequency", freq_str_hz, strlen(freq_str_hz));
|
|
|
|
dev_iio_change_accel_sampling_freq_err:
|
|
return res;
|
|
}
|
|
|
|
void dev_iio_close(dev_iio_t *const iio) {
|
|
if (iio == NULL) {
|
|
return;
|
|
}
|
|
|
|
if (iio->fd > 0) {
|
|
close(iio->fd);
|
|
}
|
|
|
|
free(iio->name);
|
|
free(iio->path);
|
|
free(iio->dev_path);
|
|
free(iio);
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
static bool iio_matches(
|
|
const iio_filters_t *const in_filters,
|
|
dev_iio_t *const in_dev
|
|
) {
|
|
if (in_dev == NULL) {
|
|
return false;
|
|
}
|
|
|
|
const char *const name = dev_iio_get_name(in_dev);
|
|
if ((name == NULL) || (strcmp(name, in_filters->name) != 0)) {
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
|
|
// Load the kernel module
|
|
int result = syscall(__NR_finit_module, -1, "iio-trig-hrtimer", 0);
|
|
if (result == 0) {
|
|
printf("Kernel module '%s' loaded successfully.\n", "iio-trig-hrtimer");
|
|
} else {
|
|
perror("Error loading kernel module");
|
|
}
|
|
|
|
|
|
modprobe industrialio-sw-trigger
|
|
modprobe iio-trig-sysfs
|
|
modprobe iio-trig-hrtimer
|
|
|
|
# sysfs-trigger
|
|
echo 1 > /sys/bus/iio/devices/iio_sysfs_trigger/add_trigger
|
|
cat /sys/bus/iio/devices/trigger0/name # I got sysfstrig1
|
|
cd /sys/bus/iio/devices
|
|
cd iio\:device0
|
|
echo "sysfstrig1" > trigger/current_trigger
|
|
echo 1 > buffer0/in_anglvel_x_en
|
|
echo 1 > buffer0/in_anglvel_y_en
|
|
echo 1 > buffer0/in_anglvel_z_en
|
|
echo 1 > buffer0/in_accel_x_en
|
|
echo 1 > buffer0/in_accel_y_en
|
|
echo 1 > buffer0/in_accel_z_en
|
|
echo 1 > buffer0/enable
|
|
echo 1 > buffer/enable
|
|
cd ..
|
|
cd trigger0
|
|
echo 1 > trigger_now
|
|
|
|
# hrtimer
|
|
if [! -d "/home/config"]; then
|
|
mkdir /home/config
|
|
fi
|
|
|
|
mount -t configfs none /home/config
|
|
mkdir /home/config/iio/triggers/hrtimer/rogue
|
|
|
|
cd /sys/bus/iio/devices/iio\:device0
|
|
echo 1 > scan_elements/in_accel_x_en
|
|
echo 1 > scan_elements/in_accel_y_en
|
|
echo 1 > scan_elements/in_accel_z_en
|
|
echo 1 > scan_elements/in_anglvel_x_en
|
|
echo 1 > scan_elements/in_anglvel_y_en
|
|
echo 1 > scan_elements/in_anglvel_z_en
|
|
echo 1 > scan_elements/in_timestamp_en
|
|
echo "rogue" > trigger/current_trigger
|
|
echo 1 > buffer0/enable
|
|
*/
|
|
//static const char *const iio_hrtrigger_name = "iio-trig-hrtimer";
|
|
|
|
static const char *const iio_path = "/sys/bus/iio/devices/";
|
|
|
|
int dev_iio_open(
|
|
const iio_filters_t *const in_filters,
|
|
dev_iio_t **const out_dev
|
|
) {
|
|
int res = -ENOENT;
|
|
|
|
char dev_path[MAX_PATH_LEN] = "\n";
|
|
char path[MAX_PATH_LEN] = "\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 (strstr(dir->d_name, "iio_sysfs_trigger") != NULL) {
|
|
continue;
|
|
} else if (strstr(dir->d_name, "trigger") != NULL) {
|
|
continue;
|
|
}
|
|
|
|
snprintf(path, MAX_PATH_LEN - 1, "%s%s", iio_path, dir->d_name);
|
|
snprintf(dev_path, MAX_PATH_LEN - 1, "/dev/%s", dir->d_name);
|
|
|
|
// try to open the device, if it cannot be opened to go the next
|
|
const int iio_creation_res = dev_iio_create(dev_path, path, out_dev);
|
|
if (iio_creation_res != 0) {
|
|
//fprintf(stderr, "Cannot open %s, device skipped.\n", path);
|
|
continue;
|
|
}
|
|
|
|
if (!iio_matches(in_filters, *out_dev)) {
|
|
dev_iio_close(*out_dev);
|
|
continue;
|
|
}
|
|
|
|
// the device has been found
|
|
res = 0;
|
|
break;
|
|
}
|
|
closedir(d);
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
int dev_iio_has_anglvel(const dev_iio_t* iio) {
|
|
return (iio->flags & DEV_IIO_HAS_ANGLVEL) != 0;
|
|
}
|
|
|
|
int dev_iio_has_accel(const dev_iio_t* iio) {
|
|
return (iio->flags & DEV_IIO_HAS_ACCEL) != 0;
|
|
}
|
|
|
|
int dev_iio_get_buffer_fd(const dev_iio_t *const iio) {
|
|
return iio->fd;
|
|
}
|