control.c

   1 /*
   2  * Copyright (C) 2014 Intel Corporation.
   3  */
   4
   5 #include <stdlib.h>
   6 #include <ctype.h>
   7 #include <fcntl.h>
   8 #include <pthread.h>
   9 #include <sys/epoll.h>
  10 #include <sys/socket.h>
  11 #include <utils/Log.h>
  12 #include <hardware/sensors.h>
  13 #include "control.h"
  14 #include "enumeration.h"
  15 #include "utils.h"
  16 #include "transform.h"
  17 #include "calibration.h"
  18 #include "description.h"
  19
  20 /* Currently active sensors count, per device */
  21 static int poll_sensors_per_dev[MAX_DEVICES];   /* poll-mode sensors */
  22 static int trig_sensors_per_dev[MAX_DEVICES];   /* trigger, event based */
  23
  24 static int device_fd[MAX_DEVICES];   /* fd on the /dev/iio:deviceX file */
  25
  26 static int poll_fd; /* epoll instance covering all enabled sensors */
  27
  28 static int active_poll_sensors; /* Number of enabled poll-mode sensors */
  29
  30 /* We use pthread condition variables to get worker threads out of sleep */
  31 static pthread_cond_t  thread_release_cond      [MAX_SENSORS];
  32 static pthread_mutex_t thread_release_mutex     [MAX_SENSORS];
  33
  34 /*
  35  * We associate tags to each of our poll set entries. These tags have the
  36  * following values:
  37  * - a iio device number if the fd is a iio character device fd
  38  * - THREAD_REPORT_TAG_BASE + sensor handle if the fd is the receiving end of a
  39  *   pipe used by a sysfs data acquisition thread
  40  *  */
  41 #define THREAD_REPORT_TAG_BASE  0x00010000
  42
  43
  44 static int enable_buffer(int dev_num, int enabled)
  45 {
  46         char sysfs_path[PATH_MAX];
  47
  48         sprintf(sysfs_path, ENABLE_PATH, dev_num);
  49
  50         /* Low level, non-multiplexed, enable/disable routine */
  51         return sysfs_write_int(sysfs_path, enabled);
  52 }
  53
  54
  55 static void setup_trigger (int s, const char* trigger_val)
  56 {
  57         char sysfs_path[PATH_MAX];
  58
  59         sprintf(sysfs_path, TRIGGER_PATH, sensor_info[s].dev_num);
  60
  61         if (trigger_val[0] != '\n')
  62                 ALOGI("Setting S%d (%s) trigger to %s\n", s,
  63                         sensor_info[s].friendly_name, trigger_val);
  64
  65         sysfs_write_str(sysfs_path, trigger_val);
  66
  67         sensor_info[s].selected_trigger = trigger_val;
  68 }
  69
  70
  71 void build_sensor_report_maps(int dev_num)
  72 {
  73         /*
  74          * Read sysfs files from a iio device's scan_element directory, and
  75          * build a couple of tables from that data. These tables will tell, for
  76          * each sensor, where to gather relevant data in a device report, i.e.
  77          * the structure that we read from the /dev/iio:deviceX file in order to
  78          * sensor report, itself being the data that we return to Android when a
  79          * sensor poll completes. The mapping should be straightforward in the
  80          * case where we have a single sensor active per iio device but, this is
  81          * not the general case. In general several sensors can be handled
  82          * through a single iio device, and the _en, _index and _type syfs
  83          * entries all concur to paint a picture of what the structure of the
  84          * device report is.
  85          */
  86
  87         int s;
  88         int c;
  89         int n;
  90         int i;
  91         int ch_index;
  92         char* ch_spec;
  93         char spec_buf[MAX_TYPE_SPEC_LEN];
  94         struct datum_info_t* ch_info;
  95         int size;
  96         char sysfs_path[PATH_MAX];
  97         int known_channels;
  98         int offset;
  99         int channel_size_from_index[MAX_SENSORS * MAX_CHANNELS] = { 0 };
 100         int sensor_handle_from_index[MAX_SENSORS * MAX_CHANNELS] = { 0 };
 101         int channel_number_from_index[MAX_SENSORS * MAX_CHANNELS] = { 0 };
 102
 103         known_channels = 0;
 104
 105         /* For each sensor that is linked to this device */
 106         for (s=0; s<sensor_count; s++) {
 107                 if (sensor_info[s].dev_num != dev_num)
 108                         continue;
 109
 110                 i = sensor_info[s].catalog_index;
 111
 112                 /* Read channel details through sysfs attributes */
 113                 for (c=0; c<sensor_info[s].num_channels; c++) {
 114
 115                         /* Read _type file */
 116                         sprintf(sysfs_path, CHANNEL_PATH "%s",
 117                                 sensor_info[s].dev_num,
 118                                 sensor_catalog[i].channel[c].type_path);
 119
 120                         n = sysfs_read_str(sysfs_path, spec_buf,
 121                                                 sizeof(spec_buf));
 122
 123                         if (n == -1) {
 124                                         ALOGW(  "Failed to read type: %s\n",
 125                                         sysfs_path);
 126                                         continue;
 127                                 }
 128
 129                         ch_spec = sensor_info[s].channel[c].type_spec;
 130
 131                         memcpy(ch_spec, spec_buf, sizeof(spec_buf));
 132
 133                         ch_info = &sensor_info[s].channel[c].type_info;
 134
 135                         size = decode_type_spec(ch_spec, ch_info);
 136
 137                         /* Read _index file */
 138                         sprintf(sysfs_path, CHANNEL_PATH "%s",
 139                                 sensor_info[s].dev_num,
 140                                 sensor_catalog[i].channel[c].index_path);
 141
 142                         n = sysfs_read_int(sysfs_path, &ch_index);
 143
 144                         if (n == -1) {
 145                                         ALOGW(  "Failed to read index: %s\n",
 146                                                 sysfs_path);
 147                                         continue;
 148                                 }
 149
 150                         if (ch_index >= MAX_SENSORS) {
 151                                 ALOGE("Index out of bounds!: %s\n", sysfs_path);
 152                                 continue;
 153                         }
 154
 155                         /* Record what this index is about */
 156
 157                         sensor_handle_from_index [ch_index] = s;
 158                         channel_number_from_index[ch_index] = c;
 159                         channel_size_from_index  [ch_index] = size;
 160
 161                         known_channels++;
 162                 }
 163
 164                 /* Stop sampling - if we are recovering from hal restart */
 165                 enable_buffer(dev_num, 0);
 166                 setup_trigger(s, "\n");
 167
 168                 /* Turn on channels we're aware of */
 169                 for (c=0;c<sensor_info[s].num_channels; c++) {
 170                         sprintf(sysfs_path, CHANNEL_PATH "%s",
 171                                 sensor_info[s].dev_num,
 172                                 sensor_catalog[i].channel[c].en_path);
 173                         sysfs_write_int(sysfs_path, 1);
 174                 }
 175         }
 176
 177         ALOGI("Found %d channels on iio device %d\n", known_channels, dev_num);
 178
 179         /*
 180          * Now that we know which channels are defined, their sizes and their
 181          * ordering, update channels offsets within device report. Note: there
 182          * is a possibility that several sensors share the same index, with
 183          * their data fields being isolated by masking and shifting as specified
 184          * through the real bits and shift values in type attributes. This case
 185          * is not currently supported. Also, the code below assumes no hole in
 186          * the sequence of indices, so it is dependent on discovery of all
 187          * sensors.
 188          */
 189          offset = 0;
 190          for (i=0; i<MAX_SENSORS * MAX_CHANNELS; i++) {
 191                 s =     sensor_handle_from_index[i];
 192                 c =     channel_number_from_index[i];
 193                 size =  channel_size_from_index[i];
 194
 195                 if (!size)
 196                         continue;
 197
 198                 ALOGI("S%d C%d : offset %d, size %d, type %s\n",
 199                       s, c, offset, size, sensor_info[s].channel[c].type_spec);
 200
 201                 sensor_info[s].channel[c].offset        = offset;
 202                 sensor_info[s].channel[c].size          = size;
 203
 204                 offset += size;
 205          }
 206 }
 207
 208
 209 int adjust_counters (int s, int enabled)
 210 {
 211         /*
 212          * Adjust counters based on sensor enable action. Return values are:
 213          * -1 if there's an inconsistency: abort action in this case
 214          *  0 if the operation was completed and we're all set
 215          *  1 if we toggled the state of the sensor and there's work left
 216          */
 217
 218         int dev_num = sensor_info[s].dev_num;
 219         int catalog_index = sensor_info[s].catalog_index;
 220         int sensor_type = sensor_catalog[catalog_index].type;
 221
 222         /* Refcount per sensor, in terms of enable count */
 223         if (enabled) {
 224                 ALOGI("Enabling sensor %d (iio device %d: %s)\n",
 225                         s, dev_num, sensor_info[s].friendly_name);
 226
 227                 sensor_info[s].enable_count++;
 228
 229                 if (sensor_info[s].enable_count > 1)
 230                         return 0; /* The sensor was, and remains, in use */
 231
 232                 switch (sensor_type) {
 233                         case SENSOR_TYPE_MAGNETIC_FIELD:
 234                                 compass_read_data(&sensor_info[s]);
 235                                 break;
 236
 237                         case SENSOR_TYPE_GYROSCOPE:
 238                         case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
 239                                 gyro_cal_init(&sensor_info[s]);
 240                                 break;
 241                 }
 242         } else {
 243                 if (sensor_info[s].enable_count == 0)
 244                         return -1; /* Spurious disable call */
 245
 246                 ALOGI("Disabling sensor %d (iio device %d: %s)\n", s, dev_num,
 247                       sensor_info[s].friendly_name);
 248
 249                 sensor_info[s].enable_count--;
 250
 251                 if (sensor_info[s].enable_count > 0)
 252                         return 0; /* The sensor was, and remains, in use */
 253
 254                 /* Sensor disabled, lower report available flag */
 255                 sensor_info[s].report_pending = 0;
 256
 257                 if (sensor_type == SENSOR_TYPE_MAGNETIC_FIELD)
 258                         compass_store_data(&sensor_info[s]);
 259         }
 260
 261
 262         /* If uncalibrated type and pair is already active don't adjust counters */
 263         if (sensor_type == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED &&
 264                 sensor_info[sensor_info[s].pair_idx].enable_count != 0)
 265                         return 0;
 266
 267         /* We changed the state of a sensor - adjust per iio device counters */
 268
 269         /* If this is a regular event-driven sensor */
 270         if (sensor_info[s].num_channels) {
 271
 272                         if (enabled)
 273                                 trig_sensors_per_dev[dev_num]++;
 274                         else
 275                                 trig_sensors_per_dev[dev_num]--;
 276
 277                         return 1;
 278                 }
 279
 280         if (enabled) {
 281                 active_poll_sensors++;
 282                 poll_sensors_per_dev[dev_num]++;
 283                 return 1;
 284         }
 285
 286         active_poll_sensors--;
 287         poll_sensors_per_dev[dev_num]--;
 288         return 1;
 289 }
 290
 291
 292 static int get_field_count (int s)
 293 {
 294         int catalog_index = sensor_info[s].catalog_index;
 295         int sensor_type   = sensor_catalog[catalog_index].type;
 296
 297         switch (sensor_type) {
 298                 case SENSOR_TYPE_ACCELEROMETER:         /* m/s^2        */
 299                 case SENSOR_TYPE_MAGNETIC_FIELD:        /* micro-tesla  */
 300                 case SENSOR_TYPE_ORIENTATION:           /* degrees      */
 301                 case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
 302                 case SENSOR_TYPE_GYROSCOPE:             /* radians/s    */
 303                         return 3;
 304
 305                 case SENSOR_TYPE_LIGHT:                 /* SI lux units */
 306                 case SENSOR_TYPE_AMBIENT_TEMPERATURE:   /* °C          */
 307                 case SENSOR_TYPE_TEMPERATURE:           /* °C          */
 308                 case SENSOR_TYPE_PROXIMITY:             /* centimeters  */
 309                 case SENSOR_TYPE_PRESSURE:              /* hecto-pascal */
 310                 case SENSOR_TYPE_RELATIVE_HUMIDITY:     /* percent */
 311                         return 1;
 312
 313                 case SENSOR_TYPE_ROTATION_VECTOR:
 314                         return  4;
 315
 316                 default:
 317                         ALOGE("Unknown sensor type!\n");
 318                         return 0;                       /* Drop sample */
 319         }
 320 }
 321
 322
 323 static void time_add(struct timespec *out, struct timespec *in, int64_t ns)
 324 {
 325         int64_t target_ts = 1000000000LL * in->tv_sec + in->tv_nsec + ns;
 326
 327         out->tv_sec = target_ts / 1000000000;
 328         out->tv_nsec = target_ts % 1000000000;
 329 }
 330
 331
 332 static void* acquisition_routine (void* param)
 333 {
 334         /*
 335          * Data acquisition routine run in a dedicated thread, covering a single
 336          * sensor. This loop will periodically retrieve sampling data through
 337          * sysfs, then package it as a sample and transfer it to our master poll
 338          * loop through a report fd. Checks for a cancellation signal quite
 339          * frequently, as the thread may be disposed of at any time. Note that
 340          * Bionic does not provide pthread_cancel / pthread_testcancel...
 341          */
 342
 343         int s = (int) (size_t) param;
 344         int num_fields;
 345         struct sensors_event_t data = {0};
 346         int c;
 347         int ret;
 348         struct timespec entry_time;
 349         struct timespec target_time;
 350         int64_t period;
 351
 352         ALOGV("Entering data acquisition thread for sensor %d\n", s);
 353
 354         if (s < 0 || s >= sensor_count) {
 355                 ALOGE("Invalid sensor handle!\n");
 356                 return NULL;
 357         }
 358
 359         if (!sensor_info[s].sampling_rate) {
 360                 ALOGE("Zero rate in acquisition routine for sensor %d\n", s);
 361                 return NULL;
 362         }
 363
 364         num_fields = get_field_count(s);
 365
 366         /*
 367          * Each condition variable is associated to a mutex that has to be
 368          * locked by the thread that's waiting on it. We use these condition
 369          * variables to get the acquisition threads out of sleep quickly after
 370          * the sampling rate is adjusted, or the sensor is disabled.
 371          */
 372         pthread_mutex_lock(&thread_release_mutex[s]);
 373
 374         while (1) {
 375                 /* Pinpoint the moment we start sampling */
 376                 clock_gettime(CLOCK_REALTIME, &entry_time);
 377
 378                 ALOGV("Acquiring sample data for sensor %d through sysfs\n", s);
 379
 380                 /* Read values through sysfs */
 381                 for (c=0; c<num_fields; c++) {
 382                         data.data[c] = acquire_immediate_value(s, c);
 383
 384                         /* Check and honor termination requests */
 385                         if (sensor_info[s].thread_data_fd[1] == -1)
 386                                 goto exit;
 387
 388                         ALOGV("\tfield %d: %f\n", c, data.data[c]);
 389
 390                 }
 391
 392                 /* If the sample looks good */
 393                 if (sensor_info[s].ops.finalize(s, &data)) {
 394
 395                         /* Pipe it for transmission to poll loop */
 396                         ret = write(    sensor_info[s].thread_data_fd[1],
 397                                         data.data,
 398                                         num_fields * sizeof(float));
 399                 }
 400
 401                 /* Check and honor termination requests */
 402                 if (sensor_info[s].thread_data_fd[1] == -1)
 403                         goto exit;
 404
 405
 406                 period = (int64_t) (1000000000.0/ sensor_info[s].sampling_rate);
 407
 408                 time_add(&target_time, &entry_time, period);
 409
 410                 /*
 411                  * Wait until the sampling time elapses, or a rate change is
 412                  * signaled, or a thread exit is requested.
 413                  */
 414                 ret = pthread_cond_timedwait(   &thread_release_cond[s],
 415                                                 &thread_release_mutex[s],
 416                                                 &target_time);
 417
 418                 /* Check and honor termination requests */
 419                 if (sensor_info[s].thread_data_fd[1] == -1)
 420                                 goto exit;
 421         }
 422
 423 exit:
 424         ALOGV("Acquisition thread for S%d exiting\n", s);
 425         pthread_mutex_unlock(&thread_release_mutex[s]);
 426         pthread_exit(0);
 427         return NULL;
 428 }
 429
 430
 431 static void start_acquisition_thread (int s)
 432 {
 433         int incoming_data_fd;
 434         int ret;
 435
 436         struct epoll_event ev = {0};
 437
 438         ALOGV("Initializing acquisition context for sensor %d\n", s);
 439
 440         /* Create condition variable and mutex for quick thread release */
 441         ret = pthread_cond_init(&thread_release_cond[s], NULL);
 442         ret = pthread_mutex_init(&thread_release_mutex[s], NULL);
 443
 444         /* Create a pipe for inter thread communication */
 445         ret = pipe(sensor_info[s].thread_data_fd);
 446
 447         incoming_data_fd = sensor_info[s].thread_data_fd[0];
 448
 449         ev.events = EPOLLIN;
 450         ev.data.u32 = THREAD_REPORT_TAG_BASE + s;
 451
 452         /* Add incoming side of pipe to our poll set, with a suitable tag */
 453         ret = epoll_ctl(poll_fd, EPOLL_CTL_ADD, incoming_data_fd , &ev);
 454
 455         /* Create and start worker thread */
 456         ret = pthread_create(   &sensor_info[s].acquisition_thread,
 457                                 NULL,
 458                                 acquisition_routine,
 459                                 (void*) (size_t) s);
 460 }
 461
 462
 463 static void stop_acquisition_thread (int s)
 464 {
 465         int incoming_data_fd = sensor_info[s].thread_data_fd[0];
 466         int outgoing_data_fd = sensor_info[s].thread_data_fd[1];
 467
 468         ALOGV("Tearing down acquisition context for sensor %d\n", s);
 469
 470         /* Delete the incoming side of the pipe from our poll set */
 471         epoll_ctl(poll_fd, EPOLL_CTL_DEL, incoming_data_fd, NULL);
 472
 473         /* Mark the pipe ends as invalid ; that's a cheap exit flag */
 474         sensor_info[s].thread_data_fd[0] = -1;
 475         sensor_info[s].thread_data_fd[1] = -1;
 476
 477         /* Close both sides of our pipe */
 478         close(incoming_data_fd);
 479         close(outgoing_data_fd);
 480
 481         /* Stop acquisition thread and clean up thread handle */
 482         pthread_cond_signal(&thread_release_cond[s]);
 483         pthread_join(sensor_info[s].acquisition_thread, NULL);
 484
 485         /* Clean up our sensor descriptor */
 486         sensor_info[s].acquisition_thread = -1;
 487
 488         /* Delete condition variable and mutex */
 489         pthread_cond_destroy(&thread_release_cond[s]);
 490         pthread_mutex_destroy(&thread_release_mutex[s]);
 491 }
 492
 493
 494 int sensor_activate(int s, int enabled)
 495 {
 496         char device_name[PATH_MAX];
 497         struct epoll_event ev = {0};
 498         int dev_fd;
 499         int ret;
 500         int dev_num = sensor_info[s].dev_num;
 501         int is_poll_sensor = !sensor_info[s].num_channels;
 502
 503         /* If we want to activate gyro calibrated and gyro uncalibrated is activated
 504          * Deactivate gyro uncalibrated - Uncalibrated releases handler
 505          * Activate gyro calibrated     - Calibrated has handler
 506          * Reactivate gyro uncalibrated - Uncalibrated gets data from calibrated */
 507
 508         /* If we want to deactivate gyro calibrated and gyro uncalibrated is active
 509          * Deactivate gyro uncalibrated - Uncalibrated no longer gets data from handler
 510          * Deactivate gyro calibrated   - Calibrated releases handler
 511          * Reactivate gyro uncalibrated - Uncalibrated has handler */
 512
 513         if (sensor_catalog[sensor_info[s].catalog_index].type == SENSOR_TYPE_GYROSCOPE &&
 514                 sensor_info[s].pair_idx && sensor_info[sensor_info[s].pair_idx].enable_count != 0) {
 515
 516                                 sensor_activate(sensor_info[s].pair_idx, 0);
 517                                 ret = sensor_activate(s, enabled);
 518                                 sensor_activate(sensor_info[s].pair_idx, 1);
 519                                 return ret;
 520         }
 521
 522         ret = adjust_counters(s, enabled);
 523
 524         /* If the operation was neutral in terms of state, we're done */
 525         if (ret <= 0)
 526                 return ret;
 527
 528
 529         if (!is_poll_sensor) {
 530
 531                 /* Stop sampling */
 532                 enable_buffer(dev_num, 0);
 533                 setup_trigger(s, "\n");
 534
 535                 /* If there's at least one sensor enabled on this iio device */
 536                 if (trig_sensors_per_dev[dev_num]) {
 537
 538                         /* Start sampling */
 539                         setup_trigger(s, sensor_info[s].init_trigger_name);
 540                         enable_buffer(dev_num, 1);
 541                 }
 542         }
 543
 544         /*
 545          * Make sure we have a fd on the character device ; conversely, close
 546          * the fd if no one is using associated sensors anymore. The assumption
 547          * here is that the underlying driver will power on the relevant
 548          * hardware block while someone holds a fd on the device.
 549          */
 550         dev_fd = device_fd[dev_num];
 551
 552         if (!enabled) {
 553                 if (is_poll_sensor)
 554                         stop_acquisition_thread(s);
 555
 556                 if (dev_fd != -1 && !poll_sensors_per_dev[dev_num] &&
 557                         !trig_sensors_per_dev[dev_num]) {
 558                                 /*
 559                                  * Stop watching this fd. This should be a no-op
 560                                  * in case this fd was not in the poll set.
 561                                  */
 562                                 epoll_ctl(poll_fd, EPOLL_CTL_DEL, dev_fd, NULL);
 563
 564                                 close(dev_fd);
 565                                 device_fd[dev_num] = -1;
 566                         }
 567
 568                 /* If we recorded a trail of samples for filtering, delete it */
 569                 if (sensor_info[s].history) {
 570                         free(sensor_info[s].history);
 571                         sensor_info[s].history = NULL;
 572                         sensor_info[s].history_size = 0;
 573                 }
 574
 575                 return 0;
 576         }
 577
 578         if (dev_fd == -1) {
 579                 /* First enabled sensor on this iio device */
 580                 sprintf(device_name, DEV_FILE_PATH, dev_num);
 581                 dev_fd = open(device_name, O_RDONLY | O_NONBLOCK);
 582
 583                 device_fd[dev_num] = dev_fd;
 584
 585                 if (dev_fd == -1) {
 586                         ALOGE("Could not open fd on %s (%s)\n",
 587                               device_name, strerror(errno));
 588                         adjust_counters(s, 0);
 589                         return -1;
 590                 }
 591
 592                 ALOGV("Opened %s: fd=%d\n", device_name, dev_fd);
 593
 594                 if (!is_poll_sensor) {
 595
 596                         /* Add this iio device fd to the set of watched fds */
 597                         ev.events = EPOLLIN;
 598                         ev.data.u32 = dev_num;
 599
 600                         ret = epoll_ctl(poll_fd, EPOLL_CTL_ADD, dev_fd, &ev);
 601
 602                         if (ret == -1) {
 603                                 ALOGE(  "Failed adding %d to poll set (%s)\n",
 604                                         dev_fd, strerror(errno));
 605                                 return -1;
 606                         }
 607
 608                         /* Note: poll-mode fds are not readable */
 609                 }
 610         }
 611
 612         /* Ensure that on-change sensors send at least one event after enable */
 613         sensor_info[s].prev_val = -1;
 614
 615         if (is_poll_sensor)
 616                 start_acquisition_thread(s);
 617
 618         return 0;
 619 }
 620
 621
 622 static void enable_motion_trigger (int dev_num)
 623 {
 624         /*
 625          * In the ideal case, we enumerate two triggers per iio device ; the
 626          * default (periodically firing) trigger, and another one (the motion
 627          * trigger) that only fires up when motion is detected. This second one
 628          * allows for lesser energy consumption, but requires periodic sample
 629          * duplication at the HAL level for sensors that Android defines as
 630          * continuous. This "duplicate last sample" logic can only be engaged
 631          * once we got a first sample for the driver, so we start with the
 632          * default trigger when an iio device is first opened, then adjust the
 633          * trigger when we got events for all active sensors. Unfortunately in
 634          * the general case several sensors can be associated to a given iio
 635          * device, they can independently be controlled, and we have to adjust
 636          * the trigger in use at the iio device level depending on whether or
 637          * not appropriate conditions are met at the sensor level.
 638          */
 639
 640         int s;
 641         int i;
 642         int active_sensors = trig_sensors_per_dev[dev_num];
 643         int candidate[MAX_SENSORS];
 644         int candidate_count = 0;
 645
 646         if  (!active_sensors)
 647                 return;
 648
 649         /* Check that all active sensors are ready to switch */
 650
 651         for (s=0; s<MAX_SENSORS; s++)
 652                 if (sensor_info[s].dev_num == dev_num &&
 653                     sensor_info[s].enable_count &&
 654                     sensor_info[s].num_channels &&
 655                     (!sensor_info[s].motion_trigger_name[0] ||
 656                      !sensor_info[s].report_initialized)
 657                     )
 658                         return; /* Nope */
 659
 660         /* Record which particular sensors need to switch */
 661
 662         for (s=0; s<MAX_SENSORS; s++)
 663                 if (sensor_info[s].dev_num == dev_num &&
 664                     sensor_info[s].enable_count &&
 665                     sensor_info[s].num_channels &&
 666                     sensor_info[s].selected_trigger !=
 667                         sensor_info[s].motion_trigger_name)
 668                                 candidate[candidate_count++] = s;
 669
 670         if (!candidate_count)
 671                 return;
 672
 673         /* Now engage the motion trigger for sensors which aren't using it */
 674
 675         enable_buffer(dev_num, 0);
 676
 677         for (i=0; i<candidate_count; i++) {
 678                 s = candidate[i];
 679                 setup_trigger(s, sensor_info[s].motion_trigger_name);
 680         }
 681
 682         enable_buffer(dev_num, 1);
 683 }
 684
 685
 686 static int integrate_device_report(int dev_num)
 687 {
 688         int len;
 689         int s,c;
 690         unsigned char buf[MAX_SENSOR_REPORT_SIZE] = { 0 };
 691         int sr_offset;
 692         unsigned char *target;
 693         unsigned char *source;
 694         int size;
 695         int64_t ts;
 696
 697         /* There's an incoming report on the specified iio device char dev fd */
 698
 699         if (dev_num < 0 || dev_num >= MAX_DEVICES) {
 700                 ALOGE("Event reported on unexpected iio device %d\n", dev_num);
 701                 return -1;
 702         }
 703
 704         if (device_fd[dev_num] == -1) {
 705                 ALOGE("Ignoring stale report on iio device %d\n", dev_num);
 706                 return -1;
 707         }
 708
 709         ts = get_timestamp();
 710
 711         len = read(device_fd[dev_num], buf, MAX_SENSOR_REPORT_SIZE);
 712
 713         if (len == -1) {
 714                 ALOGE("Could not read report from iio device %d (%s)\n",
 715                       dev_num, strerror(errno));
 716                 return -1;
 717         }
 718
 719         ALOGV("Read %d bytes from iio device %d\n", len, dev_num);
 720
 721         /* Map device report to sensor reports */
 722
 723         for (s=0; s<MAX_SENSORS; s++)
 724                 if (sensor_info[s].dev_num == dev_num &&
 725                     sensor_info[s].enable_count) {
 726
 727                         sr_offset = 0;
 728
 729                         /* Copy data from device to sensor report buffer */
 730                         for (c=0; c<sensor_info[s].num_channels; c++) {
 731
 732                                 target = sensor_info[s].report_buffer +
 733                                         sr_offset;
 734
 735                                 source = buf + sensor_info[s].channel[c].offset;
 736
 737                                 size = sensor_info[s].channel[c].size;
 738
 739                                 memcpy(target, source, size);
 740
 741                                 sr_offset += size;
 742                         }
 743
 744                         ALOGV("Sensor %d report available (%d bytes)\n", s,
 745                               sr_offset);
 746
 747                         sensor_info[s].report_ts = ts;
 748                         sensor_info[s].report_pending = 1;
 749                         sensor_info[s].report_initialized = 1;
 750                 }
 751
 752         /* Tentatively switch to an any-motion trigger if conditions are met */
 753         enable_motion_trigger(dev_num);
 754
 755         return 0;
 756 }
 757
 758
 759 static int propagate_sensor_report(int s, struct sensors_event_t  *data)
 760 {
 761         /* There's a sensor report pending for this sensor ; transmit it */
 762
 763         int catalog_index = sensor_info[s].catalog_index;
 764         int sensor_type   = sensor_catalog[catalog_index].type;
 765         int num_fields    = get_field_count(s);
 766         int c;
 767         unsigned char* current_sample;
 768
 769         /* If there's nothing to return... we're done */
 770         if (!num_fields)
 771                 return 0;
 772
 773
 774         /* Only return uncalibrated event if also gyro active */
 775         if (sensor_type == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED &&
 776                 sensor_info[sensor_info[s].pair_idx].enable_count != 0)
 777                         return 0;
 778
 779         memset(data, 0, sizeof(sensors_event_t));
 780
 781         data->version   = sizeof(sensors_event_t);
 782         data->sensor    = s;
 783         data->type      = sensor_type;
 784         data->timestamp = get_timestamp();
 785
 786         ALOGV("Sample on sensor %d (type %d):\n", s, sensor_type);
 787
 788         current_sample = sensor_info[s].report_buffer;
 789
 790         /* If this is a poll sensor */
 791         if (!sensor_info[s].num_channels) {
 792                 /* Use the data provided by the acquisition thread */
 793                 ALOGV("Reporting data from worker thread for S%d\n", s);
 794                 memcpy(data->data, current_sample, num_fields * sizeof(float));
 795                 return 1;
 796         }
 797
 798         /* Convert the data into the expected Android-level format */
 799         for (c=0; c<num_fields; c++) {
 800
 801                 data->data[c] = sensor_info[s].ops.transform
 802                                                         (s, c, current_sample);
 803
 804                 ALOGV("\tfield %d: %f\n", c, data->data[c]);
 805                 current_sample += sensor_info[s].channel[c].size;
 806         }
 807
 808         /*
 809          * The finalize routine, in addition to its late sample processing duty,
 810          * has the final say on whether or not the sample gets sent to Android.
 811          */
 812         return sensor_info[s].ops.finalize(s, data);
 813 }
 814
 815
 816 static void synthetize_duplicate_samples (void)
 817 {
 818         /*
 819          * Some sensor types (ex: gyroscope) are defined as continuously firing
 820          * by Android, despite the fact that we can be dealing with iio drivers
 821          * that only report events for new samples. For these we generate
 822          * reports periodically, duplicating the last data we got from the
 823          * driver. This is not necessary for polling sensors.
 824          */
 825
 826         int s;
 827         int64_t current_ts;
 828         int64_t target_ts;
 829         int64_t period;
 830
 831         for (s=0; s<sensor_count; s++) {
 832
 833                 /* Ignore disabled sensors */
 834                 if (!sensor_info[s].enable_count)
 835                         continue;
 836
 837                 /* If the sensor can generate duplicates, leave it alone */
 838                 if (!(sensor_info[s].quirks & QUIRK_TERSE_DRIVER) &&
 839                         sensor_info[s].selected_trigger !=
 840                         sensor_info[s].motion_trigger_name)
 841                         continue;
 842
 843                 /* If we haven't seen a sample, there's nothing to duplicate */
 844                 if (!sensor_info[s].report_initialized)
 845                         continue;
 846
 847                 /* If a sample was recently buffered, leave it alone too */
 848                 if (sensor_info[s].report_pending)
 849                         continue;
 850
 851                 /* We also need a valid sampling rate to be configured */
 852                 if (!sensor_info[s].sampling_rate)
 853                         continue;
 854
 855                 period = (int64_t) (1000000000.0/ sensor_info[s].sampling_rate);
 856
 857                 current_ts = get_timestamp();
 858                 target_ts = sensor_info[s].report_ts + period;
 859
 860                 if (target_ts <= current_ts) {
 861                         /* Mark the sensor for event generation */
 862                         sensor_info[s].report_ts = current_ts;
 863                         sensor_info[s].report_pending = 1;
 864                 }
 865         }
 866 }
 867
 868
 869 static void integrate_thread_report (uint32_t tag)
 870 {
 871         int s = tag - THREAD_REPORT_TAG_BASE;
 872         int len;
 873         int expected_len;
 874
 875         expected_len = get_field_count(s) * sizeof(float);
 876
 877         len = read(sensor_info[s].thread_data_fd[0],
 878                    sensor_info[s].report_buffer,
 879                    expected_len);
 880
 881         if (len == expected_len) {
 882                 sensor_info[s].report_ts = get_timestamp();
 883                 sensor_info[s].report_pending = 1;
 884         }
 885 }
 886
 887
 888 static int get_poll_wait_timeout (void)
 889 {
 890         /*
 891          * Compute an appropriate timeout value, in ms, for the epoll_wait
 892          * call that's going to await for iio device reports and incoming
 893          * reports from our sensor sysfs data reader threads.
 894          */
 895
 896         int s;
 897         int64_t target_ts = INT64_MAX;
 898         int64_t ms_to_wait;
 899         int64_t period;
 900
 901         /*
 902          * Check if have have to deal with "terse" drivers that only send events
 903          * when there is motion, despite the fact that the associated Android
 904          * sensor type is continuous rather than on-change. In that case we have
 905          * to duplicate events. Check deadline for the nearest upcoming event.
 906          */
 907         for (s=0; s<sensor_count; s++)
 908                 if (sensor_info[s].enable_count &&
 909                     ((sensor_info[s].quirks & QUIRK_TERSE_DRIVER) ||
 910                       sensor_info[s].selected_trigger ==
 911                       sensor_info[s].motion_trigger_name) &&
 912                      sensor_info[s].sampling_rate) {
 913                         period = (int64_t) (1000000000.0 /
 914                                                 sensor_info[s].sampling_rate);
 915
 916                         if (sensor_info[s].report_ts + period < target_ts)
 917                                 target_ts = sensor_info[s].report_ts + period;
 918                 }
 919
 920         /* If we don't have such a driver to deal with */
 921         if (target_ts == INT64_MAX)
 922                 return -1; /* Infinite wait */
 923
 924         ms_to_wait = (target_ts - get_timestamp()) / 1000000;
 925
 926         /* If the target timestamp is already behind us, don't wait */
 927         if (ms_to_wait < 1)
 928                 return 0;
 929
 930         return ms_to_wait;
 931 }
 932
 933
 934 int sensor_poll(struct sensors_event_t* data, int count)
 935 {
 936         int s;
 937         int i;
 938         int nfds;
 939         struct epoll_event ev[MAX_DEVICES];
 940         int returned_events;
 941         int event_count;
 942         int uncal_start;
 943
 944         /* Get one or more events from our collection of sensors */
 945
 946 return_available_sensor_reports:
 947
 948         returned_events = 0;
 949
 950         /* Check our sensor collection for available reports */
 951         for (s=0; s<sensor_count && returned_events < count; s++)
 952                 if (sensor_info[s].report_pending) {
 953                         event_count = 0;
 954                         /* Lower flag */
 955                         sensor_info[s].report_pending = 0;
 956
 957                         /* Report this event if it looks OK */
 958                         event_count = propagate_sensor_report(s, &data[returned_events]);
 959
 960                         /* Duplicate only if both cal & uncal are active */
 961                         if (sensor_catalog[sensor_info[s].catalog_index].type == SENSOR_TYPE_GYROSCOPE &&
 962                                         sensor_info[s].pair_idx && sensor_info[sensor_info[s].pair_idx].enable_count != 0) {
 963                                         struct gyro_cal* gyro_data = (struct gyro_cal*) sensor_info[s].cal_data;
 964
 965                                         memcpy(&data[returned_events + event_count], &data[returned_events],
 966                                                         sizeof(struct sensors_event_t) * event_count);
 967
 968                                         uncal_start = returned_events + event_count;
 969                                         for (i = 0; i < event_count; i++) {
 970                                                 data[uncal_start + i].type = SENSOR_TYPE_GYROSCOPE_UNCALIBRATED;
 971                                                 data[uncal_start + i].sensor = sensor_info[s].pair_idx;
 972
 973                                                 data[uncal_start + i].data[0] = data[returned_events + i].data[0] + gyro_data->bias_x;
 974                                                 data[uncal_start + i].data[1] = data[returned_events + i].data[1] + gyro_data->bias_y;
 975                                                 data[uncal_start + i].data[2] = data[returned_events + i].data[2] + gyro_data->bias_z;
 976
 977                                                 data[uncal_start + i].uncalibrated_gyro.bias[0] = gyro_data->bias_x;
 978                                                 data[uncal_start + i].uncalibrated_gyro.bias[1] = gyro_data->bias_y;
 979                                                 data[uncal_start + i].uncalibrated_gyro.bias[2] = gyro_data->bias_z;
 980                                         }
 981                                         event_count <<= 1;
 982                         }
 983                         sensor_info[sensor_info[s].pair_idx].report_pending = 0;
 984                         returned_events += event_count;
 985                         /*
 986                          * If the sample was deemed invalid or unreportable,
 987                          * e.g. had the same value as the previously reported
 988                          * value for a 'on change' sensor, silently drop it.
 989                          */
 990                 }
 991
 992         if (returned_events)
 993                 return returned_events;
 994
 995 await_event:
 996
 997         ALOGV("Awaiting sensor data\n");
 998
 999         nfds = epoll_wait(poll_fd, ev, MAX_DEVICES, get_poll_wait_timeout());
1000
1001         if (nfds == -1) {
1002                 ALOGE("epoll_wait returned -1 (%s)\n", strerror(errno));
1003                 goto await_event;
1004         }
1005
1006         /* Synthetize duplicate samples if needed */
1007         synthetize_duplicate_samples();
1008
1009         ALOGV("%d fds signalled\n", nfds);
1010
1011         /* For each of the signalled sources */
1012         for (i=0; i<nfds; i++)
1013                 if (ev[i].events == EPOLLIN)
1014                         switch (ev[i].data.u32) {
1015                                 case 0 ... MAX_DEVICES-1:
1016                                         /* Read report from iio char dev fd */
1017                                         integrate_device_report(ev[i].data.u32);
1018                                         break;
1019
1020                                 case THREAD_REPORT_TAG_BASE ...
1021                                      THREAD_REPORT_TAG_BASE + MAX_SENSORS-1:
1022                                         /* Get report from acquisition thread */
1023                                         integrate_thread_report(ev[i].data.u32);
1024                                         break;
1025
1026                                 default:
1027                                         ALOGW("Unexpected event source!\n");
1028                                         break;
1029                         }
1030
1031         goto return_available_sensor_reports;
1032 }
1033
1034
1035 int sensor_set_delay(int s, int64_t ns)
1036 {
1037         /* Set the rate at which a specific sensor should report events */
1038
1039         /* See Android sensors.h for indication on sensor trigger modes */
1040
1041         char sysfs_path[PATH_MAX];
1042         char avail_sysfs_path[PATH_MAX];
1043         int dev_num             =       sensor_info[s].dev_num;
1044         int i                   =       sensor_info[s].catalog_index;
1045         const char *prefix      =       sensor_catalog[i].tag;
1046         float new_sampling_rate; /* Granted sampling rate after arbitration   */
1047         float cur_sampling_rate; /* Currently used sampling rate              */
1048         int per_sensor_sampling_rate;
1049         int per_device_sampling_rate;
1050         int32_t min_delay = sensor_desc[s].minDelay;
1051         float max_supported_rate = (min_delay != 0 && min_delay != -1) ? (1000000.0f / min_delay) : 0;
1052         char freqs_buf[100];
1053         char* cursor;
1054         int n;
1055         float sr;
1056
1057         if (!ns) {
1058                 ALOGE("Rejecting zero delay request on sensor %d\n", s);
1059                 return -EINVAL;
1060         }
1061
1062         new_sampling_rate = 1000000000LL/ns;
1063
1064         /*
1065          * Artificially limit ourselves to 1 Hz or higher. This is mostly to
1066          * avoid setting up the stage for divisions by zero.
1067          */
1068         if (new_sampling_rate < 1)
1069                 new_sampling_rate = 1;
1070
1071         if (max_supported_rate &&
1072                 new_sampling_rate > max_supported_rate) {
1073                 new_sampling_rate = max_supported_rate;
1074         }
1075
1076         sensor_info[s].sampling_rate = new_sampling_rate;
1077
1078         /* If we're dealing with a poll-mode sensor */
1079         if (!sensor_info[s].num_channels) {
1080                 /* Interrupt current sleep so the new sampling gets used */
1081                 pthread_cond_signal(&thread_release_cond[s]);
1082                 return 0;
1083         }
1084
1085         sprintf(sysfs_path, SENSOR_SAMPLING_PATH, dev_num, prefix);
1086
1087         if (sysfs_read_float(sysfs_path, &cur_sampling_rate) != -1) {
1088                 per_sensor_sampling_rate = 1;
1089                 per_device_sampling_rate = 0;
1090         } else {
1091                 per_sensor_sampling_rate = 0;
1092
1093                 sprintf(sysfs_path, DEVICE_SAMPLING_PATH, dev_num);
1094
1095                 if (sysfs_read_float(sysfs_path, &cur_sampling_rate) != -1)
1096                         per_device_sampling_rate = 1;
1097                 else
1098                         per_device_sampling_rate = 0;
1099         }
1100
1101         if (!per_sensor_sampling_rate && !per_device_sampling_rate) {
1102                 ALOGE("No way to adjust sampling rate on sensor %d\n", s);
1103                 return -ENOSYS;
1104         }
1105
1106         /* Coordinate with others active sensors on the same device, if any */
1107         if (per_device_sampling_rate)
1108                 for (n=0; n<sensor_count; n++)
1109                         if (n != s && sensor_info[n].dev_num == dev_num &&
1110                             sensor_info[n].num_channels &&
1111                             sensor_info[n].enable_count &&
1112                             sensor_info[n].sampling_rate > new_sampling_rate)
1113                                 new_sampling_rate= sensor_info[n].sampling_rate;
1114
1115         /* Check if we have contraints on allowed sampling rates */
1116
1117         sprintf(avail_sysfs_path, DEVICE_AVAIL_FREQ_PATH, dev_num);
1118
1119         if (sysfs_read_str(avail_sysfs_path, freqs_buf, sizeof(freqs_buf)) > 0){
1120                 cursor = freqs_buf;
1121
1122                 /* Decode allowed sampling rates string, ex: "10 20 50 100" */
1123
1124                 /* While we're not at the end of the string */
1125                 while (*cursor && cursor[0]) {
1126
1127                         /* Decode a single value */
1128                         sr = strtod(cursor, NULL);
1129
1130                         /* If this matches the selected rate, we're happy */
1131                         if (new_sampling_rate == sr)
1132                                 break;
1133
1134                         /*
1135                          * If we reached a higher value than the desired rate,
1136                          * adjust selected rate so it matches the first higher
1137                          * available one and stop parsing - this makes the
1138                          * assumption that rates are sorted by increasing value
1139                          * in the allowed frequencies string.
1140                          */
1141                         if (sr > new_sampling_rate) {
1142                                 new_sampling_rate = sr;
1143                                 break;
1144                         }
1145
1146                         /* Skip digits */
1147                         while (cursor[0] && !isspace(cursor[0]))
1148                                 cursor++;
1149
1150                         /* Skip spaces */
1151                         while (cursor[0] && isspace(cursor[0]))
1152                                         cursor++;
1153                 }
1154         }
1155
1156
1157         if (max_supported_rate &&
1158                 new_sampling_rate > max_supported_rate) {
1159                 new_sampling_rate = max_supported_rate;
1160         }
1161
1162
1163         /* If the desired rate is already active we're all set */
1164         if (new_sampling_rate == cur_sampling_rate)
1165                 return 0;
1166
1167         ALOGI("Sensor %d sampling rate set to %g\n", s, new_sampling_rate);
1168
1169         if (trig_sensors_per_dev[dev_num])
1170                 enable_buffer(dev_num, 0);
1171
1172         sysfs_write_float(sysfs_path, new_sampling_rate);
1173
1174         if (trig_sensors_per_dev[dev_num])
1175                 enable_buffer(dev_num, 1);
1176
1177         return 0;
1178 }
1179
1180
1181 int allocate_control_data (void)
1182 {
1183         int i;
1184
1185         for (i=0; i<MAX_DEVICES; i++)
1186                 device_fd[i] = -1;
1187
1188         poll_fd = epoll_create(MAX_DEVICES);
1189
1190         if (poll_fd == -1) {
1191                 ALOGE("Can't create epoll instance for iio sensors!\n");
1192                 return -1;
1193         }
1194
1195         return poll_fd;
1196 }
1197
1198
1199 void delete_control_data (void)
1200 {
1201 }