libsensors/sensors.c

   1 /*
   2  *  (c) Copyright Bosch Sensortec GmbH 2011
   3
   4  *   Redistribution and use in source and binary forms, with or without
   5  *   modification, are permitted provided that the following conditions are
   6  *   met:
   7  *
   8  *       * Redistributions of source code must retain the above copyright
   9  *          notice, this list of conditions and the following disclaimer.
  10  *       * Redistributions in binary form must reproduce the above
  11  *          copyright notice, this list of conditions and the following
  12  *          disclaimer in the documentation and/or other materials provided
  13  *          with the distribution.
  14  *       * Neither the name of the author nor the names of its
  15  *          contributors may be used to endorse or promote products derived
  16  *          from this software without specific prior written permission.
  17  *
  18  *
  19  *   THIS SOFTWARE IS PROVIDED "AS IS" AND ANY EXPRESS OR IMPLIED
  20  *   WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
  21  *   MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT
  22  *   ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
  23  *   BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  24  *   CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  25  *   SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
  26  *   BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
  27  *   WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
  28  *   OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
  29  *   IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  30  */
  31
  32 #include <stdint.h>
  33 #include <string.h>
  34 #include <unistd.h>
  35 #include <stdlib.h>
  36 #include <stdio.h>
  37 #include <fcntl.h>
  38 #include <errno.h>
  39 #include <dirent.h>
  40 #include <math.h>
  41 #include <poll.h>
  42 #include <pthread.h>
  43 #include <linux/input.h>
  44
  45 #include <cutils/atomic.h>
  46 #include <cutils/log.h>
  47 #include <hardware/sensors.h>
  48
  49 #define DEBUG_SENSOR            0
  50
  51 #define CONVERT                     (GRAVITY_EARTH / 156.0f)
  52 #define SENSOR_NAME             "hdaps"
  53 #define INPUT_DIR               "/dev/input"
  54 #define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))
  55 #define ID_ACCELERATION         (SENSORS_HANDLE_BASE + 0)
  56
  57 #define AMIN(a,b) (((a)<(fabs(b)))?(a):(b))
  58 #define SQUARE(x)       ((x)*(x))
  59 #define COS_ASIN(m,x)   (sqrt(SQUARE(m)-SQUARE(AMIN(m,x))))
  60 #define COS_ASIN_2D(m,x,y)      (COS_ASIN(m,x)*COS_ASIN(m,y)/(m))
  61
  62 struct sensors_poll_context_t {
  63         struct sensors_poll_device_t device;
  64         int fd;
  65         char class_path[256];
  66 };
  67
  68 static int common__close(struct hw_device_t *dev) {
  69         struct sensors_poll_context_t *ctx = (struct sensors_poll_context_t *) dev;
  70         if (ctx) {
  71                 free(ctx);
  72         }
  73
  74         return 0;
  75 }
  76
  77 static int device__activate(struct sensors_poll_device_t *dev, int handle,
  78                 int enabled) {
  79
  80         return 0;
  81 }
  82
  83 static int device__set_delay(struct sensors_poll_device_t *device, int handle,
  84                 int64_t ns) {
  85
  86         return 0;
  87
  88 }
  89
  90 static int device__poll(struct sensors_poll_device_t *device,
  91                 sensors_event_t* data, int count) {
  92
  93         struct input_event event;
  94         int ret;
  95         struct sensors_poll_context_t *dev =
  96                         (struct sensors_poll_context_t *) device;
  97
  98         if (dev->fd < 0)
  99                 return 0;
 100
 101         while (1) {
 102
 103                 ret = read(dev->fd, &event, sizeof(event));
 104
 105 #ifdef DEBUG_SENSOR
 106                 LOGD("read event %d - %d - %d\n", event.type, event.code, event.value);
 107 #endif
 108                 if (event.type == EV_ABS) {
 109                         switch (event.code) {
 110                         // Android imagines the device in portrait mode, but hdaps measures in landscape mode,
 111                         // so swap x and y axis!
 112                         case ABS_X:
 113                                 data->acceleration.x = (float) event.value * CONVERT;
 114                                 break;
 115                         case ABS_Y:
 116                                 data->acceleration.y = -(float) event.value * CONVERT;
 117                                 break;
 118                         }
 119                 } else if (event.type == EV_SYN) {
 120                         data->timestamp = (int64_t) ((int64_t) event.time.tv_sec
 121                                         * 1000000000 + (int64_t) event.time.tv_usec * 1000);
 122                         // hdaps doesn't have z-axis, so simulate it by rotation matrix solution
 123                         data ->acceleration.z
 124                                         = COS_ASIN_2D(GRAVITY_EARTH, data->acceleration.x, data->acceleration.y);
 125                         data->sensor = ID_ACCELERATION;
 126                         data->type = SENSOR_TYPE_ACCELEROMETER;
 127                         data->acceleration.status = SENSOR_STATUS_ACCURACY_HIGH;
 128                         return 1;
 129                 }
 130         }
 131
 132         return -errno;
 133 }
 134
 135 static int sensor_get_class_path(struct sensors_poll_context_t *dev) {
 136         char *dirname = "/sys/class/input";
 137         char buf[256];
 138         int res;
 139         DIR *dir;
 140         struct dirent *de;
 141         int fd = -1;
 142         int found = 0;
 143
 144         dir = opendir(dirname);
 145         if (dir == NULL)
 146                 return -1;
 147
 148         while ((de = readdir(dir))) {
 149                 if (strncmp(de->d_name, "input", strlen("input")) != 0) {
 150                         continue;
 151                 }
 152
 153                 sprintf(dev->class_path, "%s/%s", dirname, de->d_name);
 154                 snprintf(buf, sizeof(buf), "%s/name", dev->class_path);
 155
 156                 fd = open(buf, O_RDONLY);
 157                 if (fd < 0) {
 158                         continue;
 159                 }
 160                 if ((res = read(fd, buf, sizeof(buf))) < 0) {
 161                         close(fd);
 162                         continue;
 163                 }
 164                 buf[res - 1] = '\0';
 165                 if (strcmp(buf, SENSOR_NAME) == 0) {
 166                         found = 1;
 167                         close(fd);
 168                         break;
 169                 }
 170
 171                 close(fd);
 172                 fd = -1;
 173         }
 174         closedir(dir);
 175
 176         if (found) {
 177                 return 0;
 178         } else {
 179                 *dev->class_path = '\0';
 180                 return -1;
 181         }
 182
 183 }
 184
 185 static int open_input_device(void) {
 186         char *filename;
 187         int fd;
 188         DIR *dir;
 189         struct dirent *de;
 190         char name[80];
 191         char devname[256];
 192         dir = opendir(INPUT_DIR);
 193         if (dir == NULL)
 194                 return -1;
 195
 196         strcpy(devname, INPUT_DIR);
 197         filename = devname + strlen(devname);
 198         *filename++ = '/';
 199
 200         while ((de = readdir(dir))) {
 201                 if (de->d_name[0] == '.' && (de->d_name[1] == '\0' || (de->d_name[1]
 202                                 == '.' && de->d_name[2] == '\0')))
 203                         continue;
 204                 strcpy(filename, de->d_name);
 205                 fd = open(devname, O_RDONLY);
 206                 if (fd < 0) {
 207                         continue;
 208                 }
 209
 210                 if (ioctl(fd, EVIOCGNAME(sizeof(name) - 1), &name) < 1) {
 211                         name[0] = '\0';
 212                 }
 213
 214                 if (!strcmp(name, SENSOR_NAME)) {
 215 #ifdef DEBUG_SENSOR
 216                         LOGI("devname is %s \n", devname);
 217 #endif
 218                 } else {
 219                         close(fd);
 220                         continue;
 221                 }
 222                 closedir(dir);
 223
 224                 return fd;
 225
 226         }
 227         closedir(dir);
 228
 229         return -1;
 230 }
 231
 232 static const struct sensor_t sSensorList[] = { //
 233                 { .name = "HDAPS accelerometer", //
 234                                 .vendor = "Linux kernel", //
 235                                 .version = 1, //
 236                                 .handle = ID_ACCELERATION, //
 237                                 .type = SENSOR_TYPE_ACCELEROMETER, //
 238                                 .maxRange = (GRAVITY_EARTH * 6.0f), //
 239                                 .resolution = (GRAVITY_EARTH * 6.0f) / 1024.0f, //
 240                                 .power = 0.84f, //
 241                                 .reserved = {}, //
 242                                 }, //
 243                 };
 244
 245 static int open_sensors(const struct hw_module_t* module, const char* name,
 246                 struct hw_device_t** device);
 247
 248 static int sensors__get_sensors_list(struct sensors_module_t* module,
 249                 struct sensor_t const** list) {
 250         *list = sSensorList;
 251
 252         return ARRAY_SIZE(sSensorList);
 253 }
 254
 255 static struct hw_module_methods_t sensors_module_methods = {
 256                 .open = open_sensors };
 257
 258 const struct sensors_module_t HAL_MODULE_INFO_SYM = { //
 259                 .common = { //
 260                                         .tag = HARDWARE_MODULE_TAG, //
 261                                         .version_major = 1, //
 262                                         .version_minor = 0, //
 263                                         .id = SENSORS_HARDWARE_MODULE_ID, //
 264                                         .name = "hdaps accelerometer sensor", //
 265                                         .author = "Stefan Seidel", //
 266                                         .methods = &sensors_module_methods, //
 267                                         .dso = NULL, //
 268                                         .reserved = {}, //
 269                                 },//
 270                                 get_sensors_list : sensors__get_sensors_list //
 271                 };
 272
 273 static int open_sensors(const struct hw_module_t* module, const char* name,
 274                 struct hw_device_t** device) {
 275         int status = -EINVAL;
 276
 277         struct sensors_poll_context_t *dev = malloc(
 278                         sizeof(struct sensors_poll_context_t));
 279         memset(&dev->device, 0, sizeof(struct sensors_poll_device_t));
 280
 281         dev->device.common.tag = HARDWARE_DEVICE_TAG;
 282         dev->device.common.version = 0;
 283         dev->device.common.module = (struct hw_module_t*) module;
 284         dev->device.common.close = common__close;
 285         dev->device.activate = device__activate;
 286         dev->device.setDelay = device__set_delay;
 287         dev->device.poll = device__poll;
 288
 289         if (sensor_get_class_path(dev) < 0) {
 290                 LOGE("g sensor get class path error \n");
 291         } else {
 292                 dev->fd = open_input_device();
 293                 *device = &dev->device.common;
 294                 status = 0;
 295         }
 296
 297         return status;
 298 }