espresso-common: libsensors: sync with replicant

Change-Id: Idcb9a57af699833c504f236692a6bdb1ef33c98e

3 files changed, 780 insertions, 233 deletions

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 <contact@paulk.fr>
  *
  * 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 <stdlib.h>
-#include <unistd.h>
-#include <stdint.h>
+#include <stdio.h>
 #include <fcntl.h>
 #include <errno.h>
-#include <dirent.h>
-#include <sys/stat.h>
-#include <linux/ioctl.h>
+#include <poll.h>
 #include <linux/input.h>
 
+#include <hardware/sensors.h>
+#include <hardware/hardware.h>
+
 #define LOG_TAG "geomagneticd"
 #include <utils/Log.h>
 
-/*
- * 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 <contact@paulk.fr>
+ *
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+#include <poll.h>
+#include <linux/input.h>
+
+#include <hardware/sensors.h>
+#include <hardware/hardware.h>
+
+#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 <contact@paulk.fr>
+ *
+ * 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 <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdlib.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <dirent.h>
+#include <linux/ioctl.h>
+#include <linux/input.h>
+#include <linux/uinput.h>
+
+#define LOG_TAG "geomagneticd"
+#include <utils/Log.h>
+
+#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;
+}