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