extras/accelerometer/accelerometer.c

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