From af6a75109ed68e89998f221ba27fe15980f01714 Mon Sep 17 00:00:00 2001 From: Paul Kocialkowski Date: Mon, 15 Jun 2015 16:22:16 +0200 Subject: espresso-common: libsensors: sync with replicant Change-Id: Idcb9a57af699833c504f236692a6bdb1ef33c98e --- libsensors/geomagneticd/geomagneticd.c | 619 ++++++++++++++++++++------------- libsensors/geomagneticd/geomagneticd.h | 59 ++++ libsensors/geomagneticd/input.c | 335 ++++++++++++++++++ 3 files changed, 780 insertions(+), 233 deletions(-) create mode 100644 libsensors/geomagneticd/geomagneticd.h create mode 100644 libsensors/geomagneticd/input.c (limited to 'libsensors/geomagneticd') diff --git a/libsensors/geomagneticd/geomagneticd.c b/libsensors/geomagneticd/geomagneticd.c index 918edbf..61c4795 100644 --- a/libsensors/geomagneticd/geomagneticd.c +++ b/libsensors/geomagneticd/geomagneticd.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2013 Paul Kocialkowski + * Copyright (C) 2013 Paul Kocialkowski * * This program is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by @@ -16,353 +16,506 @@ */ #include -#include -#include +#include #include #include -#include -#include -#include +#include #include +#include +#include + #define LOG_TAG "geomagneticd" #include -/* - * This is a very intuitive implementation of what's going on with p5100/p3100 - * geomagneticd daemon. It seemed that geomagneticd sets an offset so that - * the biggest value (after setting the offset) is 45µT or negative -45µT. - * On the X axis, it happens more often to find the max around 40µT/-40µT. - * The reference offsets I used were: 5005 420432 1153869, and we're getting - * pretty close to this with that implementation. - * - */ +#include "geomagneticd.h" + +// This geomagnetic daemon is in charge of finding the correct calibration +// offsets to apply to the YAS530 magnetic field sensor. +// This is done by finding raw data extrema (minimum and maximum) for each axis +// and calculating the offset so that these values are -45uT and 45uT. /* - * Input + * Config */ -int input_open(char *name) +int geomagneticd_config_read(struct geomagneticd_data *data) { - DIR *d; - struct dirent *di; - - char input_name[80] = { 0 }; - char path[PATH_MAX]; - char *c; - int fd; + char buffer[100] = { 0 }; + int config_fd = -1; int rc; - if (name == NULL) + if (data == NULL) return -EINVAL; - d = opendir("/dev/input"); - if (d == NULL) - return -1; + config_fd = open(GEOMAGNETICD_CONFIG_PATH, O_RDONLY); + if (config_fd < 0) { + ALOGE("%s: Unable to open config", __func__); + goto error; + } - while ((di = readdir(d))) { - if (di == NULL || strcmp(di->d_name, ".") == 0 || strcmp(di->d_name, "..") == 0) - continue; + rc = read(config_fd, buffer, sizeof(buffer)); + if (rc <= 0) { + ALOGE("%s: Unable to read config", __func__); + goto error; + } - snprintf(path, PATH_MAX, "/dev/input/%s", di->d_name); - fd = open(path, O_RDONLY); - if (fd < 0) - continue; + rc = sscanf(buffer, "%d,%d,%d,%d,%d,%d,%d", + &data->hard_offsets[0], &data->hard_offsets[1], &data->hard_offsets[2], + &data->calib_offsets[0], &data->calib_offsets[1], &data->calib_offsets[2], + &data->accuracy); + if (rc != 7) { + ALOGE("%s: Unable to parse config", __func__); + goto error; + } - rc = ioctl(fd, EVIOCGNAME(sizeof(input_name) - 1), &input_name); - if (rc < 0) - continue; + rc = 0; + goto complete; - c = strstr((char *) &input_name, "\n"); - if (c != NULL) - *c = '\0'; +error: + rc = -1; - if (strcmp(input_name, name) == 0) - return fd; - else - close(fd); - } +complete: + if (config_fd >= 0) + close(config_fd); - return -1; + return rc; } -int sysfs_path_prefix(char *name, char *path_prefix) +int geomagneticd_config_write(struct geomagneticd_data *data) { - DIR *d; - struct dirent *di; - - char input_name[80] = { 0 }; - char path[PATH_MAX]; - char *c; - int fd; + char buffer[100] = { 0 }; + int config_fd = -1; + int rc; - if (name == NULL || path_prefix == NULL) + if (data == NULL) return -EINVAL; - d = opendir("/sys/class/input"); - if (d == NULL) - return -1; + config_fd = open(GEOMAGNETICD_CONFIG_BACKUP_PATH, O_WRONLY | O_TRUNC | O_CREAT, 0644); + if (config_fd < 0) { + ALOGE("%s: Unable to open config", __func__); + goto error; + } - while ((di = readdir(d))) { - if (di == NULL || strcmp(di->d_name, ".") == 0 || strcmp(di->d_name, "..") == 0) - continue; + sprintf(buffer, "%d,%d,%d,%d,%d,%d,%d", + data->hard_offsets[0], data->hard_offsets[1], data->hard_offsets[2], + data->calib_offsets[0], data->calib_offsets[1], data->calib_offsets[2], + data->accuracy); - snprintf(path, PATH_MAX, "/sys/class/input/%s/name", di->d_name); + rc = write(config_fd, buffer, strlen(buffer) + 1); + if (rc < (int) strlen(buffer) + 1) { + ALOGE("%s: Unable to write config", __func__); + goto error; + } - fd = open(path, O_RDONLY); - if (fd < 0) - continue; + rename(GEOMAGNETICD_CONFIG_BACKUP_PATH, GEOMAGNETICD_CONFIG_PATH); - read(fd, &input_name, sizeof(input_name)); - close(fd); + rc = 0; + goto complete; - c = strstr((char *) &input_name, "\n"); - if (c != NULL) - *c = '\0'; +error: + rc = -1; - if (strcmp(input_name, name) == 0) { - snprintf(path_prefix, PATH_MAX, "/sys/class/input/%s", di->d_name); - return 0; - } - } +complete: + if (config_fd >= 0) + close(config_fd); - return -1; + return rc; } /* - * Geomagneticd + * Offsets */ -int offset_read(char *path, int *hard_offset, int *calib_offset, int *accuracy) +int geomagneticd_offsets_read(struct geomagneticd_data *data) { - char buf[100] = { 0 }; - int fd; + char buffer[100] = { 0 }; + int offsets_fd = -1; int rc; - fd = open(path, O_RDONLY); - if (fd < 0) - return -1; + if (data == NULL) + return -EINVAL; - rc = read(fd, &buf, sizeof(buf)); - close(fd); - if (rc <= 0) - return -1; + offsets_fd = open(data->path_offsets, O_RDONLY); + if (offsets_fd < 0) { + ALOGE("%s: Unable to open offsets", __func__); + goto error; + } - rc = sscanf(buf, "%d %d %d %d %d %d %d", - &hard_offset[0], &hard_offset[1], &hard_offset[2], - &calib_offset[0], &calib_offset[1], &calib_offset[2], accuracy); + rc = read(offsets_fd, buffer, sizeof(buffer)); + if (rc <= 0) { + ALOGE("%s: Unable to read offsets", __func__); + goto error; + } - if (rc != 7) - return -1; + rc = sscanf(buffer, "%d %d %d %d %d %d %d", + &data->hard_offsets[0], &data->hard_offsets[1], &data->hard_offsets[2], + &data->calib_offsets[0], &data->calib_offsets[1], &data->calib_offsets[2], + &data->accuracy); + if (rc != 7) { + ALOGE("%s: Unable to parse offsets", __func__); + goto error; + } - return 0; + rc = 0; + goto complete; + +error: + rc = -1; + +complete: + if (offsets_fd >= 0) + close(offsets_fd); + + return rc; } -int offset_write(char *path, int *hard_offset, int *calib_offset, int accuracy) +int geomagneticd_offsets_write(struct geomagneticd_data *data) { - char buf[100] = { 0 }; - int fd; + char buffer[100] = { 0 }; + int offsets_fd = -1; int rc; - sprintf(buf, "%d %d %d %d %d %d %d\n", - hard_offset[0], hard_offset[1], hard_offset[2], - calib_offset[0], calib_offset[1], calib_offset[2], accuracy); + if (data == NULL) + return -EINVAL; - fd = open(path, O_WRONLY); - if (fd < 0) - return -1; + offsets_fd = open(data->path_offsets, O_WRONLY); + if (offsets_fd < 0) { + ALOGE("%s: Unable to open offsets", __func__); + goto error; + } - write(fd, buf, strlen(buf) + 1); - close(fd); + sprintf(buffer, "%d %d %d %d %d %d %d\n", + data->hard_offsets[0], data->hard_offsets[1], data->hard_offsets[2], + data->calib_offsets[0], data->calib_offsets[1], data->calib_offsets[2], + data->accuracy); - return 0; + rc = write(offsets_fd, buffer, strlen(buffer) + 1); + if (rc < (int) strlen(buffer) + 1) { + ALOGE("%s: Unable to write offsets", __func__); + goto error; + } + + rc = 0; + goto complete; + +error: + rc = -1; + +complete: + if (offsets_fd >= 0) + close(offsets_fd); + + return rc; } -int yas_cfg_read(int *hard_offset, int *calib_offset, int *accuracy) +int geomagneticd_offsets_init(struct geomagneticd_data *data) { - char buf[100] = { 0 }; - int fd; - int rc; + int count; + int i; - fd = open("/data/system/yas.cfg", O_RDONLY); - if (fd < 0) - return -1; + if (data == NULL) + return -EINVAL; - rc = read(fd, &buf, sizeof(buf)); - close(fd); - if (rc <= 0) - return -1; + count = sizeof(data->hard_offsets) / sizeof(int); - rc = sscanf(buf, "%d,%d,%d,%d,%d,%d,%d", - &hard_offset[0], &hard_offset[1], &hard_offset[2], - &calib_offset[0], &calib_offset[1], &calib_offset[2], accuracy); + // 0x7f is an invalid value for hard offsets + for (i = 0; i < count; i++) + data->hard_offsets[i] = 0x7f; - if (rc != 7) - return -1; + count = sizeof(data->calib_offsets) / sizeof(int); + + // 0x0x7fffffff is an invalid value for calib offsets + for (i = 0; i < count; i++) + data->calib_offsets[i] = 0x7fffffff; return 0; } -int yas_cfg_write(int *hard_offset, int *calib_offset, int accuracy) +int geomagneticd_offsets_check(struct geomagneticd_data *data) { - char buf[100] = { 0 }; - int fd; - int rc; + int count; + int i; - fd = open("/data/system/yas-backup.cfg", O_WRONLY | O_TRUNC | O_CREAT, S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP); - if (fd < 0) - return -1; + if (data == NULL) + return -EINVAL; - sprintf(buf, "%d,%d,%d,%d,%d,%d,%d\n", - hard_offset[0], hard_offset[1], hard_offset[2], - calib_offset[0], calib_offset[1], calib_offset[2], accuracy); + count = sizeof(data->hard_offsets) / sizeof(int); - write(fd, buf, strlen(buf) + 1); - close(fd); + // 0x7f is an invalid value for hard offsets + for (i = 0; i < count; i++) + if (data->hard_offsets[i] == 0x7f) + return 0; - chmod("/data/system/yas-backup.cfg", 0644); - rename("/data/system/yas-backup.cfg", "/data/system/yas.cfg"); + count = sizeof(data->calib_offsets) / sizeof(int); - return 0; + // 0x0x7fffffff is an invalid value for calib offsets + for (i = 0; i < count; i++) + if (data->calib_offsets[i] == 0x7fffffff) + return 0; + + return 1; } -int main(int argc, char *argv[]) +/* + * Geomagneticd + */ + +int geomagneticd_magnetic_extrema_init(struct geomagneticd_data *data) { - struct input_event event; + int count; + int i; - char path[PATH_MAX] = { 0 }; - char path_offset[PATH_MAX] = { 0 }; + if (data == NULL) + return -EINVAL; + + count = sizeof(data->calib_offsets) / sizeof(int); + + // Approximate the previous extrema from the calib offsets + for (i = 0; i < count; i++) { + data->magnetic_extrema[0][i] = -(data->calib_offsets[i] + 45) * 1000 + 5000; + data->magnetic_extrema[1][i] = (data->calib_offsets[i] + 45) * 1000 - 5000; + } - int offset_fd; - int input_fd; + return 0; +} + +int geomagneticd_magnetic_extrema(struct geomagneticd_data *data, int index, + int value) +{ + if (data == NULL || index < 0 || index >= 3) + return -EINVAL; - int max_coeff[3] = { 40, 45, 45 }; - int hard_offset[3] = { 0 }; - int calib_offset[3] = { 0 }; - int accuracy = 0; + if (value == 0) + return 0; - int axis_min[3] = { 0 }; - int axis_max[3] = { 0 }; - int axis_calib[3] = { 0 }; + // Update the extrema from the current value if needed + if (value < data->magnetic_extrema[0][index] || data->magnetic_extrema[0][index] == 0) + data->magnetic_extrema[0][index] = value; + if (value > data->magnetic_extrema[1][index] || data->magnetic_extrema[1][index] == 0) + data->magnetic_extrema[1][index] = value; - int x, y, z; + return 0; +} +int geomagneticd_calib_offsets(struct geomagneticd_data *data) +{ + int calib_offsets[3]; + int offsets[2]; + int update; + int count; int rc; int i; - /* - * Wait for something to be ready and properly report the hard coeff. - * Without that, the hard coeff are reported to be around 127. - */ + if (data == NULL) + return -EINVAL; - ALOGD("Geomagneticd start"); + // Calculating the offset is only meaningful when enough values were + // obtained. There is no need to calculate it too often either. + if (data->count % 10 != 0) + return 0; - input_fd = input_open("geomagnetic_raw"); - if (input_fd < 0) - goto sleep_loop; + update = 0; - rc = sysfs_path_prefix("geomagnetic_raw", &path); - if (rc < 0) - goto sleep_loop; + count = sizeof(data->calib_offsets) / sizeof(int); - snprintf(path_offset, PATH_MAX, "%s/offsets", path); + // Calculate the calib offset for each axis to have values in [-45;45] uT + for (i = 0; i < count; i++) { + offsets[0] = data->magnetic_extrema[0][i] + 45 * 1000; + offsets[1] = data->magnetic_extrema[1][i] - 45 * 1000; + calib_offsets[i] = (offsets[0] + offsets[1]) / 2; - for (i=0 ; i < 3 ; i++) { - axis_min[i] = 0; - axis_max[i] = 0; - calib_offset[i] = 0; + if (calib_offsets[i] != data->calib_offsets[i]) { + data->calib_offsets[i] = calib_offsets[i]; + update = 1; + } } - ALOGD("Reading config"); - - rc = yas_cfg_read(&hard_offset, &calib_offset, &accuracy); - if (rc == 0) { - ALOGD("Setting initial offsets: %d %d %d, %d %d %d", hard_offset[0], hard_offset[1], hard_offset[2], calib_offset[0], calib_offset[1], calib_offset[2]); - - offset_write(path_offset, &hard_offset, &calib_offset, accuracy); + if (update) { + data->accuracy = 1; - for (i=0 ; i < 3 ; i++) { - axis_min[i] = - calib_offset[i] - max_coeff[i] * 1000; - axis_max[i] = calib_offset[i] + max_coeff[i] * 1000; - axis_calib[i] = calib_offset[i]; + rc = geomagneticd_offsets_write(data); + if (rc < 0) { + ALOGE("%s: Unable to write offsets", __func__); + return -1; } - } else { - offset_read(path_offset, &hard_offset, &calib_offset, &accuracy); - ALOGD("Reading initial offsets: %d %d %d", hard_offset[0], hard_offset[1], hard_offset[2]); - for (i=0 ; i < 3 ; i++) { - axis_min[i] = 0; - axis_max[i] = 0; - calib_offset[i] = 0; + rc = geomagneticd_config_write(data); + if (rc < 0) { + ALOGE("%s: Unable to write config", __func__); + return -1; } } -loop: - while (1) { - read(input_fd, &event, sizeof(event)); - - if (event.type == EV_SYN) { - for (i=0 ; i < 3 ; i++) { - if (-axis_min[i] < axis_max[i]) { - axis_calib[i] = axis_max[i] - max_coeff[i] * 1000; - } else { - axis_calib[i] = axis_min[i] + max_coeff[i] * 1000; - } + return 0; +} - axis_calib[i] = axis_calib[i] < 0 ? -axis_calib[i] : axis_calib[i]; +int geomagneticd_poll(struct geomagneticd_data *data) +{ + struct input_event input_event; + struct pollfd poll_fd; + int rc; - if (axis_calib[i] != calib_offset[i]) { - calib_offset[i] = axis_calib[i]; - accuracy = 1; + if (data == NULL) + return -EINVAL; - offset_write(path_offset, &hard_offset, &calib_offset, accuracy); - yas_cfg_write(&hard_offset, &calib_offset, accuracy); - } + if (data->input_fd < 0) + return -1; -// printf("axis_calib[%d]=%d\n", i, axis_calib[i]); - } + memset(&poll_fd, 0, sizeof(poll_fd)); + poll_fd.fd = data->input_fd; + poll_fd.events = POLLIN; - if (hard_offset[0] == 127 && hard_offset[1] == 127 && hard_offset[2] == 127) { - offset_read(path_offset, &hard_offset, &calib_offset, &accuracy); + while (1) { + rc = poll(&poll_fd, 1, -1); + if (rc < 0) { + ALOGE("%s: poll failure", __func__); + goto error; + } - if (hard_offset[0] != 127 || hard_offset[1] != 127 || hard_offset[2] != 127) { - ALOGD("Reading offsets: %d %d %d", hard_offset[0], hard_offset[1], hard_offset[2]); - yas_cfg_write(&hard_offset, &calib_offset, accuracy); - } - } + if (!(poll_fd.revents & POLLIN)) + continue; + + poll_fd.revents = 0; + + rc = read(data->input_fd, &input_event, sizeof(input_event)); + if (rc < (int) sizeof(input_event)) { + ALOGE("%s: Unable to read input event", __func__); + continue; } - if(event.type == EV_ABS) { - switch (event.code) { + // Update the extrema from the current value + if(input_event.type == EV_ABS) { + switch (input_event.code) { case ABS_X: - x = event.value; - if (x < axis_min[0]) - axis_min[0] = x; - if (x > axis_max[0]) - axis_max[0] = x; + geomagneticd_magnetic_extrema(data, 0, input_event.value); break; case ABS_Y: - y = event.value; - if (y < axis_min[1]) - axis_min[1] = y; - if (y > axis_max[1]) - axis_max[1] = y; + geomagneticd_magnetic_extrema(data, 1, input_event.value); break; case ABS_Z: - z = event.value; - if (z < axis_min[2]) - axis_min[2] = z; - if (z > axis_max[2]) - axis_max[2] = z; + geomagneticd_magnetic_extrema(data, 2, input_event.value); break; } } + + if (input_event.type == EV_SYN) { + // Sometimes, the hard offsets cannot be read at startup + // so we need to do it now + if (!geomagneticd_offsets_check(data)) { + rc = geomagneticd_offsets_read(data); + if (rc < 0) { + ALOGE("%s: Unable to read offsets", __func__); + continue; + } + + // Most likely, the calib offset will be invalid + if (geomagneticd_offsets_check(data)) { + data->accuracy = 1; + geomagneticd_magnetic_extrema_init(data); + } + + rc = geomagneticd_config_write(data); + if (rc < 0) { + ALOGE("%s: Unable to write config", __func__); + continue; + } + } + + data->count++; + + rc = geomagneticd_calib_offsets(data); + if (rc < 0) { + ALOGE("%s: Unable to calib offsets", __func__); + continue; + } + } } -sleep_loop: - while (1) { - sleep(3600); + rc = 0; + goto complete; + +error: + rc = -1; + +complete: + return rc; +} + +int main(int argc, char *argv[]) +{ + struct geomagneticd_data *geomagneticd_data = NULL; + char path[PATH_MAX] = { 0 }; + int input_fd = -1; + int rc; + + geomagneticd_data = (struct geomagneticd_data *) + calloc(1, sizeof(struct geomagneticd_data)); + + input_fd = input_open("geomagnetic_raw"); + if (input_fd < 0) { + ALOGE("%s: Unable to open input", __func__); + goto error; } - return 0; + rc = sysfs_path_prefix("geomagnetic_raw", (char *) &path); + if (rc < 0 || path[0] == '\0') { + ALOGE("%s: Unable to open sysfs", __func__); + goto error; + } + + snprintf(geomagneticd_data->path_offsets, PATH_MAX, "%s/offsets", path); + + geomagneticd_data->input_fd = input_fd; + + geomagneticd_offsets_init(geomagneticd_data); + + // Attempt to read the offsets from the config + rc = geomagneticd_config_read(geomagneticd_data); + if (rc < 0 || !geomagneticd_offsets_check(geomagneticd_data)) { + // Read the offsets from the driver + rc = geomagneticd_offsets_read(geomagneticd_data); + if (rc < 0) { + ALOGE("%s: Unable to read offsets", __func__); + goto error; + } + + // Most likely, the calib offset will be invalid and the hard + // offset may be invalid as well + if (geomagneticd_offsets_check(geomagneticd_data)) { + geomagneticd_data->accuracy = 1; + geomagneticd_magnetic_extrema_init(geomagneticd_data); + } + } else { + // Get the magnetic extrema from the config's offsets + geomagneticd_magnetic_extrema_init(geomagneticd_data); + + rc = geomagneticd_offsets_write(geomagneticd_data); + if (rc < 0) { + ALOGE("%s: Unable to write offsets", __func__); + goto error; + } + } + + rc = geomagneticd_poll(geomagneticd_data); + if (rc < 0) + goto error; + + rc = 0; + goto complete; + +error: + while (1) + sleep(3600); + + rc = 1; + +complete: + if (input_fd >= 0) + close(input_fd); + + if (geomagneticd_data != NULL) + free(geomagneticd_data); + + return rc; } diff --git a/libsensors/geomagneticd/geomagneticd.h b/libsensors/geomagneticd/geomagneticd.h new file mode 100644 index 0000000..cd57eb9 --- /dev/null +++ b/libsensors/geomagneticd/geomagneticd.h @@ -0,0 +1,59 @@ +/* + * Copyright (C) 2013 Paul Kocialkowski + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include +#include + +#include +#include + +#ifndef _GEOMAGNETICD_H_ +#define _GEOMAGNETICD_H_ + +#define GEOMAGNETICD_CONFIG_PATH "/data/system/yas.cfg" +#define GEOMAGNETICD_CONFIG_BACKUP_PATH "/data/system/yas-backup.cfg" + +struct geomagneticd_data { + int magnetic_extrema[2][3]; + int hard_offsets[3]; + int calib_offsets[3]; + int accuracy; + + int input_fd; + char path_offsets[PATH_MAX]; + + int count; +}; + +/* + * Input + */ + +void input_event_set(struct input_event *event, int type, int code, int value); +long int timestamp(struct timeval *time); +long int input_timestamp(struct input_event *event); +int uinput_rel_create(const char *name); +void uinput_destroy(int uinput_fd); +int input_open(char *name); +int sysfs_path_prefix(char *name, char *path_prefix); +int sysfs_value_read(char *path); +int sysfs_value_write(char *path, int value); +int sysfs_string_read(char *path, char *buffer, size_t length); +int sysfs_string_write(char *path, char *buffer, size_t length); + +#endif diff --git a/libsensors/geomagneticd/input.c b/libsensors/geomagneticd/input.c new file mode 100644 index 0000000..7e9ca4a --- /dev/null +++ b/libsensors/geomagneticd/input.c @@ -0,0 +1,335 @@ +/* + * Copyright (C) 2013 Paul Kocialkowski + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define LOG_TAG "geomagneticd" +#include + +#include "geomagneticd.h" + +void input_event_set(struct input_event *event, int type, int code, int value) +{ + if (event == NULL) + return; + + memset(event, 0, sizeof(struct input_event)); + + event->type = type, + event->code = code; + event->value = value; + + gettimeofday(&event->time, NULL); +} + +long int timestamp(struct timeval *time) +{ + if (time == NULL) + return -1; + + return time->tv_sec * 1000000000LL + time->tv_usec * 1000; +} + +long int input_timestamp(struct input_event *event) +{ + if (event == NULL) + return -1; + + return timestamp(&event->time); +} + +int uinput_rel_create(const char *name) +{ + struct uinput_user_dev uinput_dev; + int uinput_fd; + int rc; + + if (name == NULL) + return -1; + + uinput_fd = open("/dev/uinput", O_WRONLY | O_NONBLOCK); + if (uinput_fd < 0) { + ALOGE("%s: Unable to open uinput device", __func__); + goto error; + } + + memset(&uinput_dev, 0, sizeof(uinput_dev)); + + strncpy(uinput_dev.name, name, sizeof(uinput_dev.name)); + uinput_dev.id.bustype = BUS_I2C; + uinput_dev.id.vendor = 0; + uinput_dev.id.product = 0; + uinput_dev.id.version = 0; + + rc = 0; + rc |= ioctl(uinput_fd, UI_SET_EVBIT, EV_REL); + rc |= ioctl(uinput_fd, UI_SET_RELBIT, REL_X); + rc |= ioctl(uinput_fd, UI_SET_RELBIT, REL_Y); + rc |= ioctl(uinput_fd, UI_SET_RELBIT, REL_Z); + rc |= ioctl(uinput_fd, UI_SET_EVBIT, EV_SYN); + + if (rc < 0) { + ALOGE("%s: Unable to set uinput bits", __func__); + goto error; + } + + rc = write(uinput_fd, &uinput_dev, sizeof(uinput_dev)); + if (rc < 0) { + ALOGE("%s: Unable to write uinput device", __func__); + goto error; + } + + rc = ioctl(uinput_fd, UI_DEV_CREATE); + if (rc < 0) { + ALOGE("%s: Unable to create uinput device", __func__); + goto error; + } + + usleep(3000); + + return uinput_fd; + +error: + if (uinput_fd >= 0) + close(uinput_fd); + + return -1; +} + +void uinput_destroy(int uinput_fd) +{ + if (uinput_fd < 0) + return; + + ioctl(uinput_fd, UI_DEV_DESTROY); +} + +int input_open(char *name) +{ + DIR *d; + struct dirent *di; + + char input_name[80] = { 0 }; + char path[PATH_MAX]; + char *c; + int fd; + int rc; + + if (name == NULL) + return -EINVAL; + + d = opendir("/dev/input"); + if (d == NULL) + return -1; + + while ((di = readdir(d))) { + if (di == NULL || strcmp(di->d_name, ".") == 0 || strcmp(di->d_name, "..") == 0) + continue; + + snprintf(path, PATH_MAX, "/dev/input/%s", di->d_name); + fd = open(path, O_RDWR | O_NONBLOCK); + if (fd < 0) + continue; + + rc = ioctl(fd, EVIOCGNAME(sizeof(input_name) - 1), &input_name); + if (rc < 0) + continue; + + c = strstr((char *) &input_name, "\n"); + if (c != NULL) + *c = '\0'; + + if (strcmp(input_name, name) == 0) + return fd; + else + close(fd); + } + + return -1; +} + +int sysfs_path_prefix(char *name, char *path_prefix) +{ + DIR *d; + struct dirent *di; + + char input_name[80] = { 0 }; + char path[PATH_MAX]; + char *c; + int fd; + + if (name == NULL || path_prefix == NULL) + return -EINVAL; + + d = opendir("/sys/class/input"); + if (d == NULL) + return -1; + + while ((di = readdir(d))) { + if (di == NULL || strcmp(di->d_name, ".") == 0 || strcmp(di->d_name, "..") == 0) + continue; + + snprintf(path, PATH_MAX, "/sys/class/input/%s/name", di->d_name); + + fd = open(path, O_RDONLY); + if (fd < 0) + continue; + + read(fd, &input_name, sizeof(input_name)); + close(fd); + + c = strstr((char *) &input_name, "\n"); + if (c != NULL) + *c = '\0'; + + if (strcmp(input_name, name) == 0) { + snprintf(path_prefix, PATH_MAX, "/sys/class/input/%s", di->d_name); + return 0; + } + } + + return -1; +} + +int sysfs_value_read(char *path) +{ + char buffer[100]; + int value; + int fd = -1; + int rc; + + if (path == NULL) + return -1; + + fd = open(path, O_RDONLY); + if (fd < 0) + goto error; + + rc = read(fd, &buffer, sizeof(buffer)); + if (rc <= 0) + goto error; + + value = atoi(buffer); + goto complete; + +error: + value = -1; + +complete: + if (fd >= 0) + close(fd); + + return value; +} + +int sysfs_value_write(char *path, int value) +{ + char buffer[100]; + int fd = -1; + int rc; + + if (path == NULL) + return -1; + + fd = open(path, O_WRONLY); + if (fd < 0) + goto error; + + snprintf((char *) &buffer, sizeof(buffer), "%d\n", value); + + rc = write(fd, buffer, strlen(buffer)); + if (rc < (int) strlen(buffer)) + goto error; + + rc = 0; + goto complete; + +error: + rc = -1; + +complete: + if (fd >= 0) + close(fd); + + return rc; +} + +int sysfs_string_read(char *path, char *buffer, size_t length) +{ + int fd = -1; + int rc; + + if (path == NULL || buffer == NULL || length == 0) + return -1; + + fd = open(path, O_RDONLY); + if (fd < 0) + goto error; + + rc = read(fd, buffer, length); + if (rc <= 0) + goto error; + + rc = 0; + goto complete; + +error: + rc = -1; + +complete: + if (fd >= 0) + close(fd); + + return rc; +} + +int sysfs_string_write(char *path, char *buffer, size_t length) +{ + int fd = -1; + int rc; + + if (path == NULL || buffer == NULL || length == 0) + return -1; + + fd = open(path, O_WRONLY); + if (fd < 0) + goto error; + + rc = write(fd, buffer, length); + if (rc <= 0) + goto error; + + rc = 0; + goto complete; + +error: + rc = -1; + +complete: + if (fd >= 0) + close(fd); + + return rc; +} -- cgit v1.1