w500_sensor.c

   1 /*
   2  * w500_sensor.c
   3  *
   4  *      Created on: 28.05.2012
   5  *      Author: Marek Vasut <marex@denx.de>
   6  *      Licensed under GPLv2 or later
   7  */
   8
   9 #define LOG_TAG "W500Sensors"
  10
  11 #include <linux/types.h>
  12 #include <linux/input.h>
  13 #include <fcntl.h>
  14 #include <cutils/sockets.h>
  15 #include <cutils/log.h>
  16 #include <cutils/native_handle.h>
  17 #include <dirent.h>
  18 #include <math.h>
  19 #include <hardware/sensors.h>
  20
  21 #define DRIVER_DESC                     "Acer BMA150 accelerometer"
  22 #define SENS_COUNT                      3
  23 #define SENS_LIGHT                      0
  24 #define SENS_ROTATE                     1
  25 #define SENS_ACCEL                      2
  26 #define ID_AMBIENT_LIGHT                (SENSORS_HANDLE_BASE + SENS_LIGHT)
  27 #define ID_ROTATION_VECTOR              (SENSORS_HANDLE_BASE + SENS_ROTATE)
  28 #define ID_ACCELEROMETER                (SENSORS_HANDLE_BASE + SENS_ACCEL)
  29
  30 #define VALID_LIGHT                     (1 << 0)
  31 #define VALID_ROTATE                    (1 << 1)
  32 #define VALID_ACCEL                     (1 << 2)
  33
  34 #define VALID_ROTATE_X                  (1 << 0)
  35 #define VALID_ROTATE_Y                  (1 << 1)
  36 #define VALID_ROTATE_Z                  (1 << 2)
  37 #define VALID_ROTATE_MASK               (7 << 0)
  38
  39 struct sensor_context {
  40         struct sensors_poll_device_t device;
  41         int accel_fd;
  42
  43         struct timespec delay;
  44
  45         uint32_t sent;
  46         uint32_t valid;
  47
  48         int light_data;
  49         int orientation_data[3];
  50         uint8_t orientation_valid;
  51 };
  52
  53 static int open_accel_sensor(void)
  54 {
  55         const char *dirname = "/dev/input";
  56         DIR *dir;
  57         struct dirent *de;
  58         char name[PATH_MAX];
  59         int ret;
  60         int fd;
  61
  62         fd = -1;
  63         dir = opendir(dirname);
  64         if (dir != NULL) {
  65                 /*
  66                  * Loop over all "eventXX" in /dev/input and look
  67                  * for our driver.
  68                  */
  69                 ALOGD("%s[%i] Looping over all eventXX...", __func__, __LINE__);
  70                 while ((de = readdir(dir))) {
  71                         if (de->d_name[0] != 'e')
  72                                 continue;
  73                         memset(name, 0, PATH_MAX);
  74                         snprintf(name, PATH_MAX, "%s/%s", dirname, de->d_name);
  75                         ALOGD("%s[%i] Open device %s", __func__, __LINE__, name);
  76
  77                         fd = open(name, O_RDWR);
  78                         if (fd < 0) {
  79                                 ALOGD("%s[%i] Could not open %s, %s", __func__,
  80                                         __LINE__, name, strerror(errno));
  81                                 continue;
  82                         }
  83
  84                         name[PATH_MAX - 1] = '\0';
  85                         ret = ioctl(fd, EVIOCGNAME(PATH_MAX - 1), &name);
  86                         if (ret < 1) {
  87                                 ALOGD("%s[%i] Could not get device name for "
  88                                         "%s, %s\n", __func__, __LINE__,
  89                                         name, strerror(errno));
  90                                 name[0] = '\0';
  91                         }
  92
  93                         ALOGI("%s[%i] Testing device %s",
  94                                         __func__, __LINE__, name);
  95
  96                         if (!strcmp(name, DRIVER_DESC)) {
  97                                 ALOGI("%s[%i] Found device %s",
  98                                         __func__, __LINE__, name);
  99                                 break;
 100                         }
 101
 102                         close(fd);
 103                 }
 104
 105                 ALOGD("%s[%i] stop loop and closing directory",
 106                         __func__, __LINE__);
 107                 closedir(dir);
 108         }
 109
 110         return fd;
 111 }
 112
 113 static int context__activate(struct sensors_poll_device_t *dev,
 114                                 int handle, int enabled)
 115 {
 116         struct sensor_context* ctx = (struct sensor_context *)dev;
 117         int fd;
 118
 119         switch (handle) {
 120         case SENS_ROTATE:
 121                 if (ctx->accel_fd >= 0)
 122                         return 0;
 123
 124                 fd = open_accel_sensor();
 125                 if (fd < 0)
 126                         return -EINVAL;
 127
 128                 ctx->accel_fd = fd;
 129                 return 0;
 130
 131         case SENS_LIGHT:
 132         case SENS_ACCEL:
 133                 /* Light and fake accel need no activation. */
 134                 return 0;
 135         }
 136
 137         return -EINVAL;
 138 }
 139
 140 static int context__setDelay(struct sensors_poll_device_t *dev,
 141                                 int handle, int64_t ns)
 142 {
 143         struct sensor_context* ctx = (struct sensor_context *)dev;
 144
 145         ctx->delay.tv_sec = 0;
 146         ctx->delay.tv_nsec = ns;
 147
 148         return 0;
 149 }
 150
 151 static int context__close(struct hw_device_t *dev)
 152 {
 153         struct sensor_context* ctx = (struct sensor_context *)dev;
 154
 155         close(ctx->accel_fd);
 156
 157         free(ctx);
 158
 159         return 0;
 160 }
 161
 162 static int poll_data(struct sensors_poll_device_t *dev, sensors_event_t *data)
 163 {
 164         struct input_event iev;
 165         struct sensor_context *ctx = (struct sensor_context *)dev;
 166         size_t res;
 167         float val;
 168         int i;
 169
 170         /* Orientation sensor */
 171         for (;;) {
 172                 res = read(ctx->accel_fd, &iev, sizeof(struct input_event));
 173                 if (res != sizeof(struct input_event))
 174                         break;
 175
 176                 if (iev.type != EV_ABS)
 177                         continue;
 178
 179                 switch (iev.code) {
 180                 case ABS_X:
 181                         ctx->orientation_data[0] = iev.value;
 182                         ctx->orientation_valid |= VALID_ROTATE_X;
 183                         break;
 184                 case ABS_Y:
 185                         ctx->orientation_data[1] = iev.value;
 186                         ctx->orientation_valid |= VALID_ROTATE_Y;
 187                         break;
 188                 case ABS_Z:
 189                         ctx->orientation_data[2] = iev.value;
 190                         ctx->orientation_valid |= VALID_ROTATE_Z;
 191                         break;
 192                 case ABS_MISC:
 193                         ctx->light_data = iev.value;
 194                         ctx->valid |= VALID_LIGHT;
 195                         return 0;
 196                 default:
 197                         break;
 198                 }
 199
 200                 if (ctx->orientation_valid == VALID_ROTATE_MASK) {
 201                         ctx->valid |= VALID_ROTATE;
 202                         return 0;
 203                 }
 204         }
 205
 206         return 0;
 207 }
 208
 209 static int craft_fake_accel(struct sensors_poll_device_t *dev, sensors_event_t *data)
 210 {
 211         struct sensor_context *ctx = (struct sensor_context *)dev;
 212
 213         const double angle = 20.0;
 214         const double cos_angle = GRAVITY_EARTH * cos(angle / M_PI);
 215         const double sin_angle = GRAVITY_EARTH * sin(angle / M_PI);
 216
 217         /*
 218          *              y=max
 219          *                |
 220          *                |
 221          *                |
 222          *                |
 223          * x=max ---------+--------- x=min
 224          *                |
 225          *                |
 226          *                |
 227          *                |
 228          *              y=min
 229          */
 230
 231         if (ctx->orientation_data[0] >= 10000) {
 232                 /* Landscape */
 233                 data->acceleration.x = 0.0f;
 234                 data->acceleration.y = cos_angle;
 235                 data->acceleration.z = sin_angle;
 236         } else if (ctx->orientation_data[0] <= -10000) {
 237                 /* Flipped landscape */
 238                 data->acceleration.x = 0.0f;
 239                 data->acceleration.y = -cos_angle;
 240                 data->acceleration.z = -sin_angle;
 241         } else if (ctx->orientation_data[1] <= -10000) {
 242                 /* Portrait */
 243                 data->acceleration.x = cos_angle;
 244                 data->acceleration.y = 0.0f;
 245                 data->acceleration.z = sin_angle;
 246         } else if (ctx->orientation_data[1] >= 10000) {
 247                 /* Flipped portrait */
 248                 data->acceleration.x = -cos_angle;
 249                 data->acceleration.y = 0.0f;
 250                 data->acceleration.z = -sin_angle;
 251         } else {
 252                 /* No change */
 253                 return 0;
 254         }
 255
 256         return 1;
 257 }
 258
 259
 260 static int submit_sensor(struct sensors_poll_device_t *dev, sensors_event_t *data)
 261 {
 262         struct sensor_context *ctx = (struct sensor_context *)dev;
 263         int ret = 0;
 264         struct timespec t;
 265         const uint32_t end = ctx->sent;
 266         uint32_t start = end;
 267         ctx->sent = (ctx->sent + 1) % SENS_COUNT;
 268
 269         /* Get time for the event. */
 270         memset(&t, 0, sizeof(t));
 271         clock_gettime(CLOCK_MONOTONIC, &t);
 272         data->timestamp = ((int64_t)(t.tv_sec) * 1000000000LL) + t.tv_nsec;
 273         data->version = sizeof(*data);
 274
 275         do {
 276                 start = (start + 1) % SENS_COUNT;
 277
 278                 switch (ctx->valid & (1 << start)) {
 279                 case VALID_LIGHT:
 280                         data->sensor = ID_AMBIENT_LIGHT;
 281                         data->type = SENSOR_TYPE_LIGHT;
 282
 283                         data->light = ctx->light_data;
 284
 285                         ALOGD("%s[%i] LIGHT %d", __func__, __LINE__,
 286                                         ctx->light_data);
 287
 288                         ctx->valid &= ~(1 << start);
 289                         return 1;
 290                 case VALID_ROTATE:
 291                         data->sensor = ID_ROTATION_VECTOR;
 292                         data->type = SENSOR_TYPE_ROTATION_VECTOR;
 293                         data->orientation.status = SENSOR_STATUS_ACCURACY_MEDIUM;
 294
 295                         float x = ctx->orientation_data[0];
 296                         float y = ctx->orientation_data[1];
 297                         float z = ctx->orientation_data[2];
 298
 299                         data->orientation.x = x;
 300                         data->orientation.y = y;
 301                         data->orientation.z = z;
 302
 303                         ALOGD("%s[%i] ROTATE %d %d %d -> %f %f %f",
 304                                 __func__, __LINE__,
 305                                 ctx->orientation_data[0],
 306                                 ctx->orientation_data[1],
 307                                 ctx->orientation_data[2],
 308                                 data->orientation.x,
 309                                 data->orientation.y,
 310                                 data->orientation.z);
 311
 312                         ctx->valid &= ~(1 << start);
 313                         ctx->valid |= VALID_ACCEL;
 314                         return 3;
 315                 case VALID_ACCEL:
 316                         data->sensor = ID_ACCELEROMETER;
 317                         data->type = SENSOR_TYPE_ACCELEROMETER;
 318                         data->acceleration.status = SENSOR_STATUS_ACCURACY_HIGH;
 319
 320                         ret = craft_fake_accel(dev, data);
 321
 322                         ALOGD("%s[%i] ACCEL %f %f %f",
 323                                 __func__, __LINE__,
 324                                 data->acceleration.x,
 325                                 data->acceleration.y,
 326                                 data->acceleration.z);
 327
 328                         ctx->valid &= ~(1 << start);
 329                         return ret;
 330                 }
 331         } while (start != end);
 332
 333         return 0;
 334 }
 335
 336 static int context__poll(struct sensors_poll_device_t *dev, sensors_event_t *data, int count)
 337 {
 338         struct sensor_context *ctx = (struct sensor_context *)dev;
 339         int ret;
 340
 341         while (1) {
 342
 343                 poll_data(dev, data);
 344
 345                 ret = submit_sensor(dev, data);
 346                 if (ret)
 347                         return ret;
 348
 349                 nanosleep(&ctx->delay, &ctx->delay);
 350         }
 351
 352         return 0;
 353 }
 354
 355 static const struct sensor_t sensor_list[] = {
 356         [0] = {
 357                 .name           = "W500 Ambient Light sensor",
 358                 .vendor         = "Unknown",
 359                 .version        = 1,
 360                 .handle         = ID_AMBIENT_LIGHT,
 361                 .type           = SENSOR_TYPE_LIGHT,
 362                 .maxRange       = 100,
 363                 .resolution     = 1,
 364                 .power          = 1,
 365                 .minDelay       = 100,
 366         },
 367         [1] = {
 368                 .name           = "W500 Gyro sensor",
 369                 .vendor         = "BOSCH Sensortec",
 370                 .version        = 1,
 371                 .handle         = ID_ROTATION_VECTOR,
 372                 .type           = SENSOR_TYPE_ROTATION_VECTOR,
 373                 .maxRange       = 35000.0f,
 374                 .resolution     = 1.0f,
 375                 .power          = 1,
 376                 .minDelay       = 100,
 377         },
 378         [2] = {
 379                 .name           = "W500 Fake accelerometer",
 380                 .vendor         = "FooBar Technology Group",
 381                 .version        = 1,
 382                 .handle         = ID_ACCELEROMETER,
 383                 .type           = SENSOR_TYPE_ACCELEROMETER,
 384                 .maxRange       = 2.8f,
 385                 .resolution     = 1.0f/4032.0f,
 386                 .power          = 3.0f,
 387                 .minDelay       = 100,
 388         },
 389 };
 390
 391 static int sensors__get_sensors_list(struct sensors_module_t *module,
 392                                         const struct sensor_t **list)
 393 {
 394         *list = sensor_list;
 395         return sizeof(sensor_list)/sizeof(sensor_list[0]);
 396 }
 397
 398
 399 static int open_sensors(const struct hw_module_t *module, const char* id,
 400                         struct hw_device_t **device)
 401 {
 402         struct sensor_context *ctx;
 403         int fd;
 404
 405         if (strcmp(id, SENSORS_HARDWARE_POLL))
 406                 return -EINVAL;
 407
 408         /* Allocate context */
 409         ctx = malloc(sizeof(*ctx));
 410         if (!ctx)
 411                 return -ENOMEM;
 412
 413         memset(ctx, 0, sizeof(*ctx));
 414
 415         ctx->accel_fd = -1;
 416         ctx->delay.tv_sec = 0;
 417         ctx->delay.tv_nsec = 100;
 418
 419         /* Do common setup */
 420         ctx->device.common.tag = HARDWARE_DEVICE_TAG;
 421         ctx->device.common.version = 0;
 422         ctx->device.common.module = (struct hw_module_t *)module;
 423         ctx->device.common.close = context__close;
 424
 425         ctx->device.activate = context__activate;
 426         ctx->device.setDelay = context__setDelay;
 427         ctx->device.poll = context__poll;
 428
 429         *device = &ctx->device.common;
 430
 431         return 0;
 432
 433 err_light:
 434         close(ctx->accel_fd);
 435 err_accel:
 436         free(ctx);
 437         return -ENOMEM;
 438 }
 439
 440 static struct hw_module_methods_t sensors_module_methods = {
 441         .open   = open_sensors
 442 };
 443
 444 struct sensors_module_t HAL_MODULE_INFO_SYM = {
 445         .common = {
 446                 .tag            = HARDWARE_MODULE_TAG,
 447                 .version_major  = 1,
 448                 .version_minor  = 0,
 449                 .id             = SENSORS_HARDWARE_MODULE_ID,
 450                 .name           = "W500 SENSORS Module",
 451                 .author         = "Marek Vasut",
 452                 .methods        = &sensors_module_methods,
 453         },
 454         .get_sensors_list       = sensors__get_sensors_list,
 455 };