[thirdparty/systemd.git] / src / udev / accelerometer / accelerometer.c

/*
 * accelerometer - exports device orientation through property
 *
 * When an "change" event is received on an accelerometer,
 * open its device node, and from the value, as well as the previous
 * value of the property, calculate the device's new orientation,
 * and export it as ID_INPUT_ACCELEROMETER_ORIENTATION.
 *
 * Possible values are:
 * undefined
 * * normal
 * * bottom-up
 * * left-up
 * * right-up
 *
 * The property will be persistent across sessions, and the new
 * orientations can be deducted from the previous one (it allows
 * for a threshold for switching between opposite ends of the
 * orientation).
 *
 * Copyright (C) 2011 Red Hat, Inc.
 * Author:
 *   Bastien Nocera <hadess@hadess.net>
 *
 * orientation_calc() from the sensorfw package
 * Copyright (C) 2009-2010 Nokia Corporation
 * Authors:
 *   Üstün Ergenoglu <ext-ustun.ergenoglu@nokia.com>
 *   Timo Rongas <ext-timo.2.rongas@nokia.com>
 *   Lihan Guo <lihan.guo@digia.com>
 *
 * 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 2 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, write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 */

#include <stdio.h>
#include <string.h>
#include <stdbool.h>
#include <math.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <stdlib.h>
#include <unistd.h>
#include <getopt.h>
#include <limits.h>
#include <linux/limits.h>
#include <linux/input.h>

#include "libudev.h"
#include "libudev-private.h"

/* we must use this kernel-compatible implementation */
#define BITS_PER_LONG (sizeof(unsigned long) * 8)
#define NBITS(x) ((((x)-1)/BITS_PER_LONG)+1)
#define OFF(x)  ((x)%BITS_PER_LONG)
#define BIT(x)  (1UL<<OFF(x))
#define LONG(x) ((x)/BITS_PER_LONG)
#define test_bit(bit, array)    ((array[LONG(bit)] >> OFF(bit)) & 1)

static int debug = 0;

static void log_fn(struct udev *udev, int priority,
                   const char *file, int line, const char *fn,
                   const char *format, va_list args)
{
        if (debug) {
                fprintf(stderr, "%s: ", fn);
                vfprintf(stderr, format, args);
        } else {
                vsyslog(priority, format, args);
        }
}

typedef enum {
        ORIENTATION_UNDEFINED,
        ORIENTATION_NORMAL,
        ORIENTATION_BOTTOM_UP,
        ORIENTATION_LEFT_UP,
        ORIENTATION_RIGHT_UP
} OrientationUp;

static const char *orientations[] = {
        "undefined",
        "normal",
        "bottom-up",
        "left-up",
        "right-up",
        NULL
};

#define ORIENTATION_UP_UP ORIENTATION_NORMAL

#define DEFAULT_THRESHOLD 250
#define RADIANS_TO_DEGREES 180.0/M_PI
#define SAME_AXIS_LIMIT 5

#define THRESHOLD_LANDSCAPE  25
#define THRESHOLD_PORTRAIT  20

static const char *
orientation_to_string (OrientationUp o)
{
        return orientations[o];
}

static OrientationUp
string_to_orientation (const char *orientation)
{
        int i;

        if (orientation == NULL)
                return ORIENTATION_UNDEFINED;
        for (i = 0; orientations[i] != NULL; i++) {
                if (streq (orientation, orientations[i]))
                        return i;
        }
        return ORIENTATION_UNDEFINED;
}

static OrientationUp
orientation_calc (OrientationUp prev,
                  int x, int y, int z)
{
        int rotation;
        OrientationUp ret = prev;

        /* Portrait check */
        rotation = round(atan((double) x / sqrt(y * y + z * z)) * RADIANS_TO_DEGREES);

        if (abs(rotation) > THRESHOLD_PORTRAIT) {
                ret = (rotation < 0) ? ORIENTATION_LEFT_UP : ORIENTATION_RIGHT_UP;

                /* Some threshold to switching between portrait modes */
                if (prev == ORIENTATION_LEFT_UP || prev == ORIENTATION_RIGHT_UP) {
                        if (abs(rotation) < SAME_AXIS_LIMIT) {
                                ret = prev;
                        }
                }

        } else {
                /* Landscape check */
                rotation = round(atan((double) y / sqrt(x * x + z * z)) * RADIANS_TO_DEGREES);

                if (abs(rotation) > THRESHOLD_LANDSCAPE) {
                        ret = (rotation < 0) ? ORIENTATION_BOTTOM_UP : ORIENTATION_NORMAL;

                        /* Some threshold to switching between landscape modes */
                        if (prev == ORIENTATION_BOTTOM_UP || prev == ORIENTATION_NORMAL) {
                                if (abs(rotation) < SAME_AXIS_LIMIT) {
                                        ret = prev;
                                }
                        }
                }
        }

        return ret;
}

static OrientationUp
get_prev_orientation(struct udev_device *dev)
{
        const char *value;

        value = udev_device_get_property_value(dev, "ID_INPUT_ACCELEROMETER_ORIENTATION");
        if (value == NULL)
                return ORIENTATION_UNDEFINED;
        return string_to_orientation(value);
}

#define SET_AXIS(axis, code_) if (ev[i].code == code_) { if (got_##axis == 0) { axis = ev[i].value; got_##axis = true; } }

/* accelerometers */
static void test_orientation(struct udev *udev,
                             struct udev_device *dev,
                             const char *devpath)
{
        OrientationUp old, new;
        _cleanup_close_ int fd = -1;
        struct input_event ev[64];
        bool got_syn = false;
        bool got_x = false, got_y = false, got_z = false;
        int x = 0, y = 0, z = 0;
        char text[64];

        old = get_prev_orientation(dev);

        fd = open(devpath, O_RDONLY);
        if (fd < 0)
                return;

        while (1) {
                int i, r;

                r = read(fd, ev, sizeof(struct input_event) * 64);

                if (r < (int) sizeof(struct input_event))
                        return;

                for (i = 0; i < r / (int) sizeof(struct input_event); i++) {
                        if (got_syn) {
                                if (ev[i].type == EV_ABS) {
                                        SET_AXIS(x, ABS_X);
                                        SET_AXIS(y, ABS_Y);
                                        SET_AXIS(z, ABS_Z);
                                }
                        }
                        if (ev[i].type == EV_SYN && ev[i].code == SYN_REPORT)
                                got_syn = true;
                        if (got_x && got_y && got_z)
                                goto read_dev;
                }
        }

read_dev:
        new = orientation_calc(old, x, y, z);
        snprintf(text, sizeof(text),
                 "ID_INPUT_ACCELEROMETER_ORIENTATION=%s", orientation_to_string(new));
        puts(text);
}

static void help(void)
{
        printf("Usage: accelerometer [options] <device path>\n"
               "  --debug         debug to stderr\n"
               "  --help          print this help text\n\n");
}

int main (int argc, char** argv)
{
        struct udev *udev;
        struct udev_device *dev;

        static const struct option options[] = {
                { "debug", no_argument, NULL, 'd' },
                { "help", no_argument, NULL, 'h' },
                {}
        };

        char devpath[PATH_MAX];
        char *devnode;
        struct udev_enumerate *enumerate;
        struct udev_list_entry *list_entry;

        udev = udev_new();
        if (udev == NULL)
                return 1;

        log_open();
        udev_set_log_fn(udev, log_fn);

        /* CLI argument parsing */
        while (1) {
                int option;

                option = getopt_long(argc, argv, "dxh", options, NULL);
                if (option == -1)
                        break;

                switch (option) {
                case 'd':
                        debug = 1;
                        log_set_max_level(LOG_DEBUG);
                        udev_set_log_priority(udev, LOG_DEBUG);
                        break;
                case 'h':
                        help();
                        exit(0);
                default:
                        exit(1);
                }
        }

        if (argv[optind] == NULL) {
                help();
                exit(1);
        }

        /* get the device */
        snprintf(devpath, sizeof(devpath), "/sys/%s", argv[optind]);
        dev = udev_device_new_from_syspath(udev, devpath);
        if (dev == NULL) {
                fprintf(stderr, "unable to access '%s'\n", devpath);
                return 1;
        }

        /* Get the children devices and find the devnode */
        devnode = NULL;
        enumerate = udev_enumerate_new(udev);
        udev_enumerate_add_match_parent(enumerate, dev);
        udev_enumerate_scan_devices(enumerate);
        udev_list_entry_foreach(list_entry, udev_enumerate_get_list_entry(enumerate)) {
                struct udev_device *device;
                const char *node;

                device = udev_device_new_from_syspath(udev_enumerate_get_udev(enumerate),
                                                      udev_list_entry_get_name(list_entry));
                if (device == NULL)
                        continue;
                /* Already found it */
                if (devnode != NULL) {
                        udev_device_unref(device);
                        continue;
                }

                node = udev_device_get_devnode(device);
                if (node == NULL) {
                        udev_device_unref(device);
                        continue;
                }
                /* Use the event sub-device */
                if (strstr(node, "/event") == NULL) {
                        udev_device_unref(device);
                        continue;
                }

                devnode = strdup(node);
                udev_device_unref(device);
        }

        if (devnode == NULL) {
                fprintf(stderr, "unable to get device node for '%s'\n", devpath);
                return 0;
        }

        log_debug("opening accelerometer device %s\n", devnode);
        test_orientation(udev, dev, devnode);
        free(devnode);
        log_close();
        return 0;
}
Commit	Line	Data
24569e24 BN	1	/*
	2	* accelerometer - exports device orientation through property
	3	*
7236c6c4 BN	4	* When an "change" event is received on an accelerometer,
	5	* open its device node, and from the value, as well as the previous
	6	* value of the property, calculate the device's new orientation,
	7	* and export it as ID_INPUT_ACCELEROMETER_ORIENTATION.
	8	*
	9	* Possible values are:
	10	* undefined
	11	* * normal
	12	* * bottom-up
	13	* * left-up
	14	* * right-up
	15	*
	16	* The property will be persistent across sessions, and the new
	17	* orientations can be deducted from the previous one (it allows
	18	* for a threshold for switching between opposite ends of the
	19	* orientation).
	20	*
24569e24	21	* Copyright (C) 2011 Red Hat, Inc.
7236c6c4 BN	22	* Author:
7236c6c4 BN	23	* Bastien Nocera <hadess@hadess.net>
24569e24	24	*
7236c6c4	25	* orientation_calc() from the sensorfw package
24569e24 BN	26	* Copyright (C) 2009-2010 Nokia Corporation
	27	* Authors:
	28	* Üstün Ergenoglu <ext-ustun.ergenoglu@nokia.com>
	29	* Timo Rongas <ext-timo.2.rongas@nokia.com>
	30	* Lihan Guo <lihan.guo@digia.com>
	31	*
23757887 SK	32	* This program is free software; you can redistribute it and/or modify
23757887 SK	33	* it under the terms of the GNU General Public License as published by
24569e24 BN	34	* the Free Software Foundation; either version 2 of the License, or
	35	* (at your option) any later version.
	36	*
23757887 SK	37	* This program is distributed in the hope that it will be useful,
	38	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	39	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	40	* GNU General Public License for more details.
24569e24	41	*
23757887 SK	42	* You should have received a copy of the GNU General Public License along
	43	* with this program; if not, write to the Free Software Foundation, Inc.,
	44	* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
24569e24 BN	45	*/
	46
	47	#include <stdio.h>
	48	#include <string.h>
7a226206	49	#include <stdbool.h>
24569e24 BN	50	#include <math.h>
	51	#include <sys/types.h>
	52	#include <sys/stat.h>
	53	#include <fcntl.h>
	54	#include <stdlib.h>
	55	#include <unistd.h>
	56	#include <getopt.h>
	57	#include <limits.h>
	58	#include <linux/limits.h>
	59	#include <linux/input.h>
	60
	61	#include "libudev.h"
	62	#include "libudev-private.h"
	63
	64	/* we must use this kernel-compatible implementation */
	65	#define BITS_PER_LONG (sizeof(unsigned long) * 8)
	66	#define NBITS(x) ((((x)-1)/BITS_PER_LONG)+1)
	67	#define OFF(x) ((x)%BITS_PER_LONG)
	68	#define BIT(x) (1UL<<OFF(x))
	69	#define LONG(x) ((x)/BITS_PER_LONG)
	70	#define test_bit(bit, array) ((array[LONG(bit)] >> OFF(bit)) & 1)
	71
	72	static int debug = 0;
	73
	74	static void log_fn(struct udev *udev, int priority,
912541b0 KS	75	const char file, int line, const char fn,
912541b0 KS	76	const char *format, va_list args)
24569e24	77	{
912541b0 KS	78	if (debug) {
	79	fprintf(stderr, "%s: ", fn);
	80	vfprintf(stderr, format, args);
	81	} else {
	82	vsyslog(priority, format, args);
	83	}
24569e24 BN	84	}
	85
	86	typedef enum {
912541b0 KS	87	ORIENTATION_UNDEFINED,
	88	ORIENTATION_NORMAL,
	89	ORIENTATION_BOTTOM_UP,
	90	ORIENTATION_LEFT_UP,
	91	ORIENTATION_RIGHT_UP
24569e24 BN	92	} OrientationUp;
	93
	94	static const char *orientations[] = {
912541b0 KS	95	"undefined",
	96	"normal",
	97	"bottom-up",
	98	"left-up",
	99	"right-up",
	100	NULL
24569e24 BN	101	};
	102
	103	#define ORIENTATION_UP_UP ORIENTATION_NORMAL
	104
	105	#define DEFAULT_THRESHOLD 250
	106	#define RADIANS_TO_DEGREES 180.0/M_PI
	107	#define SAME_AXIS_LIMIT 5
	108
	109	#define THRESHOLD_LANDSCAPE 25
	110	#define THRESHOLD_PORTRAIT 20
	111
	112	static const char *
	113	orientation_to_string (OrientationUp o)
	114	{
912541b0	115	return orientations[o];
24569e24 BN	116	}
	117
	118	static OrientationUp
	119	string_to_orientation (const char *orientation)
	120	{
912541b0 KS	121	int i;
	122
	123	if (orientation == NULL)
	124	return ORIENTATION_UNDEFINED;
	125	for (i = 0; orientations[i] != NULL; i++) {
090be865	126	if (streq (orientation, orientations[i]))
912541b0 KS	127	return i;
	128	}
	129	return ORIENTATION_UNDEFINED;
24569e24 BN	130	}
	131
	132	static OrientationUp
	133	orientation_calc (OrientationUp prev,
912541b0	134	int x, int y, int z)
24569e24	135	{
912541b0 KS	136	int rotation;
	137	OrientationUp ret = prev;
	138
	139	/* Portrait check */
	140	rotation = round(atan((double) x / sqrt(y * y + z * z)) * RADIANS_TO_DEGREES);
	141
	142	if (abs(rotation) > THRESHOLD_PORTRAIT) {
	143	ret = (rotation < 0) ? ORIENTATION_LEFT_UP : ORIENTATION_RIGHT_UP;
	144
	145	/* Some threshold to switching between portrait modes */
	146	if (prev == ORIENTATION_LEFT_UP \|\| prev == ORIENTATION_RIGHT_UP) {
	147	if (abs(rotation) < SAME_AXIS_LIMIT) {
	148	ret = prev;
	149	}
	150	}
	151
	152	} else {
	153	/* Landscape check */
	154	rotation = round(atan((double) y / sqrt(x * x + z * z)) * RADIANS_TO_DEGREES);
	155
	156	if (abs(rotation) > THRESHOLD_LANDSCAPE) {
	157	ret = (rotation < 0) ? ORIENTATION_BOTTOM_UP : ORIENTATION_NORMAL;
	158
	159	/* Some threshold to switching between landscape modes */
	160	if (prev == ORIENTATION_BOTTOM_UP \|\| prev == ORIENTATION_NORMAL) {
	161	if (abs(rotation) < SAME_AXIS_LIMIT) {
	162	ret = prev;
	163	}
	164	}
	165	}
	166	}
	167
	168	return ret;
24569e24 BN	169	}
	170
	171	static OrientationUp
	172	get_prev_orientation(struct udev_device *dev)
	173	{
912541b0	174	const char *value;
24569e24	175
912541b0 KS	176	value = udev_device_get_property_value(dev, "ID_INPUT_ACCELEROMETER_ORIENTATION");
	177	if (value == NULL)
	178	return ORIENTATION_UNDEFINED;
	179	return string_to_orientation(value);
24569e24 BN	180	}
24569e24 BN	181
7a226206	182	#define SET_AXIS(axis, code_) if (ev[i].code == code_) { if (got_##axis == 0) { axis = ev[i].value; got_##axis = true; } }
24569e24 BN	183
	184	/* accelerometers */
	185	static void test_orientation(struct udev *udev,
912541b0 KS	186	struct udev_device *dev,
912541b0 KS	187	const char *devpath)
24569e24	188	{
912541b0	189	OrientationUp old, new;
7fd1b19b	190	_cleanup_close_ int fd = -1;
912541b0	191	struct input_event ev[64];
7a226206 ZJS	192	bool got_syn = false;
7a226206 ZJS	193	bool got_x = false, got_y = false, got_z = false;
912541b0 KS	194	int x = 0, y = 0, z = 0;
	195	char text[64];
	196
	197	old = get_prev_orientation(dev);
	198
7a226206 ZJS	199	fd = open(devpath, O_RDONLY);
7a226206 ZJS	200	if (fd < 0)
912541b0 KS	201	return;
912541b0 KS	202
912541b0	203	while (1) {
7a226206	204	int i, r;
912541b0 KS	205
	206	r = read(fd, ev, sizeof(struct input_event) * 64);
	207
7a226206	208	if (r < (int) sizeof(struct input_event))
912541b0 KS	209	return;
	210
	211	for (i = 0; i < r / (int) sizeof(struct input_event); i++) {
7a226206	212	if (got_syn) {
912541b0 KS	213	if (ev[i].type == EV_ABS) {
	214	SET_AXIS(x, ABS_X);
	215	SET_AXIS(y, ABS_Y);
	216	SET_AXIS(z, ABS_Z);
	217	}
	218	}
7a226206 ZJS	219	if (ev[i].type == EV_SYN && ev[i].code == SYN_REPORT)
7a226206 ZJS	220	got_syn = true;
912541b0 KS	221	if (got_x && got_y && got_z)
	222	goto read_dev;
	223	}
	224	}
24569e24 BN	225
24569e24 BN	226	read_dev:
912541b0	227	new = orientation_calc(old, x, y, z);
7a226206 ZJS	228	snprintf(text, sizeof(text),
7a226206 ZJS	229	"ID_INPUT_ACCELEROMETER_ORIENTATION=%s", orientation_to_string(new));
912541b0	230	puts(text);
24569e24 BN	231	}
	232
	233	static void help(void)
	234	{
912541b0 KS	235	printf("Usage: accelerometer [options] <device path>\n"
	236	" --debug debug to stderr\n"
	237	" --help print this help text\n\n");
24569e24 BN	238	}
	239
	240	int main (int argc, char** argv)
	241	{
912541b0 KS	242	struct udev *udev;
	243	struct udev_device *dev;
	244
	245	static const struct option options[] = {
	246	{ "debug", no_argument, NULL, 'd' },
	247	{ "help", no_argument, NULL, 'h' },
	248	{}
	249	};
	250
	251	char devpath[PATH_MAX];
	252	char *devnode;
912541b0 KS	253	struct udev_enumerate *enumerate;
	254	struct udev_list_entry *list_entry;
	255
	256	udev = udev_new();
	257	if (udev == NULL)
	258	return 1;
	259
baa30fbc	260	log_open();
912541b0 KS	261	udev_set_log_fn(udev, log_fn);
	262
	263	/* CLI argument parsing */
	264	while (1) {
	265	int option;
	266
	267	option = getopt_long(argc, argv, "dxh", options, NULL);
	268	if (option == -1)
	269	break;
	270
	271	switch (option) {
	272	case 'd':
	273	debug = 1;
e239cd8d KS	274	log_set_max_level(LOG_DEBUG);
e239cd8d KS	275	udev_set_log_priority(udev, LOG_DEBUG);
912541b0 KS	276	break;
	277	case 'h':
	278	help();
	279	exit(0);
	280	default:
	281	exit(1);
	282	}
	283	}
	284
	285	if (argv[optind] == NULL) {
	286	help();
	287	exit(1);
	288	}
	289
	290	/* get the device */
6ada823a	291	snprintf(devpath, sizeof(devpath), "/sys/%s", argv[optind]);
912541b0 KS	292	dev = udev_device_new_from_syspath(udev, devpath);
	293	if (dev == NULL) {
	294	fprintf(stderr, "unable to access '%s'\n", devpath);
	295	return 1;
	296	}
	297
6ada823a	298	/* Get the children devices and find the devnode */
912541b0 KS	299	devnode = NULL;
912541b0 KS	300	enumerate = udev_enumerate_new(udev);
1d010426	301	udev_enumerate_add_match_parent(enumerate, dev);
912541b0 KS	302	udev_enumerate_scan_devices(enumerate);
	303	udev_list_entry_foreach(list_entry, udev_enumerate_get_list_entry(enumerate)) {
	304	struct udev_device *device;
	305	const char *node;
	306
	307	device = udev_device_new_from_syspath(udev_enumerate_get_udev(enumerate),
	308	udev_list_entry_get_name(list_entry));
	309	if (device == NULL)
	310	continue;
	311	/* Already found it */
	312	if (devnode != NULL) {
	313	udev_device_unref(device);
	314	continue;
	315	}
	316
	317	node = udev_device_get_devnode(device);
	318	if (node == NULL) {
	319	udev_device_unref(device);
	320	continue;
	321	}
	322	/* Use the event sub-device */
	323	if (strstr(node, "/event") == NULL) {
	324	udev_device_unref(device);
	325	continue;
	326	}
	327
	328	devnode = strdup(node);
	329	udev_device_unref(device);
	330	}
	331
	332	if (devnode == NULL) {
	333	fprintf(stderr, "unable to get device node for '%s'\n", devpath);
	334	return 0;
	335	}
	336
baa30fbc	337	log_debug("opening accelerometer device %s\n", devnode);
912541b0 KS	338	test_orientation(udev, dev, devnode);
912541b0 KS	339	free(devnode);
baa30fbc	340	log_close();
912541b0	341	return 0;
24569e24	342	}