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