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         unsigned char previous_report[MAX_SENSOR_REPORT_SIZE];
 614         int sr_offset;
 615         unsigned char *target;
 616         unsigned char *source;
 617         int size;
 618         int64_t ts;
 619
 620         /* There's an incoming report on the specified iio device char dev fd */
 621
 622         if (dev_num < 0 || dev_num >= MAX_DEVICES) {
 623                 ALOGE("Event reported on unexpected iio device %d\n", dev_num);
 624                 return -1;
 625         }
 626
 627         if (device_fd[dev_num] == -1) {
 628                 ALOGE("Ignoring stale report on iio device %d\n", dev_num);
 629                 return -1;
 630         }
 631
 632         ts = get_timestamp();
 633
 634         len = read(device_fd[dev_num], buf, MAX_SENSOR_REPORT_SIZE);
 635
 636         if (len == -1) {
 637                 ALOGE("Could not read report from iio device %d (%s)\n",
 638                       dev_num, strerror(errno));
 639                 return -1;
 640         }
 641
 642         ALOGV("Read %d bytes from iio device %d\n", len, dev_num);
 643
 644         /* Map device report to sensor reports */
 645
 646         for (s=0; s<MAX_SENSORS; s++)
 647                 if (sensor_info[s].dev_num == dev_num &&
 648                     sensor_info[s].enable_count) {
 649
 650                         sr_offset = 0;
 651
 652                         /* Copy data from device to sensor report buffer */
 653                         for (c=0; c<sensor_info[s].num_channels; c++) {
 654
 655                                 target = sensor_info[s].report_buffer +
 656                                         sr_offset;
 657
 658                                 source = buf + sensor_info[s].channel[c].offset;
 659
 660                                 size = sensor_info[s].channel[c].size;
 661
 662                                 memcpy(target, source, size);
 663
 664                                 sr_offset += size;
 665                         }
 666
 667                         ALOGV("Sensor %d report available (%d bytes)\n", s,
 668                               sr_offset);
 669
 670                         sensor_info[s].report_ts = ts;
 671                         sensor_info[s].report_pending = 1;
 672                         sensor_info[s].report_initialized = 1;
 673                 }
 674
 675         return 0;
 676 }
 677
 678
 679 static int propagate_sensor_report(int s, struct sensors_event_t  *data)
 680 {
 681         /* There's a sensor report pending for this sensor ; transmit it */
 682
 683         int catalog_index = sensor_info[s].catalog_index;
 684         int sensor_type   = sensor_catalog[catalog_index].type;
 685         int num_fields    = get_field_count(s);
 686         int c;
 687         unsigned char* current_sample;
 688
 689         /* If there's nothing to return... we're done */
 690         if (!num_fields)
 691                 return 0;
 692
 693
 694         /* Only return uncalibrated event if also gyro active */
 695         if (sensor_type == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED &&
 696                 sensor_info[sensor_info[s].pair_idx].enable_count != 0)
 697                         return 0;
 698
 699         memset(data, 0, sizeof(sensors_event_t));
 700
 701         data->version   = sizeof(sensors_event_t);
 702         data->sensor    = s;
 703         data->type      = sensor_type;
 704         data->timestamp = sensor_info[s].report_ts;
 705
 706         ALOGV("Sample on sensor %d (type %d):\n", s, sensor_type);
 707
 708         current_sample = sensor_info[s].report_buffer;
 709
 710         /* If this is a poll sensor */
 711         if (!sensor_info[s].num_channels) {
 712                 /* Use the data provided by the acquisition thread */
 713                 ALOGV("Reporting data from worker thread for S%d\n", s);
 714                 memcpy(data->data, current_sample, num_fields * sizeof(float));
 715                 return 1;
 716         }
 717
 718         /* Convert the data into the expected Android-level format */
 719         for (c=0; c<num_fields; c++) {
 720
 721                 data->data[c] = sensor_info[s].ops.transform
 722                                                         (s, c, current_sample);
 723
 724                 ALOGV("\tfield %d: %f\n", c, data->data[c]);
 725                 current_sample += sensor_info[s].channel[c].size;
 726         }
 727
 728         /*
 729          * The finalize routine, in addition to its late sample processing duty,
 730          * has the final say on whether or not the sample gets sent to Android.
 731          */
 732         return sensor_info[s].ops.finalize(s, data);
 733 }
 734
 735
 736 static void synthetize_duplicate_samples (void)
 737 {
 738         /*
 739          * Some sensor types (ex: gyroscope) are defined as continuously firing
 740          * by Android, despite the fact that we can be dealing with iio drivers
 741          * that only report events for new samples. For these we generate
 742          * reports periodically, duplicating the last data we got from the
 743          * driver. This is not necessary for polling sensors.
 744          */
 745
 746         int s;
 747         int64_t current_ts;
 748         int64_t target_ts;
 749         int64_t period;
 750
 751         for (s=0; s<sensor_count; s++) {
 752
 753                 /* Ignore disabled sensors */
 754                 if (!sensor_info[s].enable_count)
 755                         continue;
 756
 757                 /* If the sensor can generate duplicates, leave it alone */
 758                 if (!(sensor_info[s].quirks & QUIRK_TERSE_DRIVER))
 759                         continue;
 760
 761                 /* If we haven't seen a sample, there's nothing to duplicate */
 762                 if (!sensor_info[s].report_initialized)
 763                         continue;
 764
 765                 /* If a sample was recently buffered, leave it alone too */
 766                 if (sensor_info[s].report_pending)
 767                         continue;
 768
 769                 /* We also need a valid sampling rate to be configured */
 770                 if (!sensor_info[s].sampling_rate)
 771                         continue;
 772
 773                 period = (int64_t) (1000000000.0/ sensor_info[s].sampling_rate);
 774
 775                 current_ts = get_timestamp();
 776                 target_ts = sensor_info[s].report_ts + period;
 777
 778                 if (target_ts <= current_ts) {
 779                         /* Mark the sensor for event generation */
 780                         sensor_info[s].report_ts = current_ts;
 781                         sensor_info[s].report_pending = 1;
 782                 }
 783         }
 784 }
 785
 786
 787 static void integrate_thread_report (uint32_t tag)
 788 {
 789         int s = tag - THREAD_REPORT_TAG_BASE;
 790         int len;
 791         int expected_len;
 792
 793         expected_len = get_field_count(s) * sizeof(float);
 794
 795         len = read(sensor_info[s].thread_data_fd[0],
 796                    sensor_info[s].report_buffer,
 797                    expected_len);
 798
 799         if (len == expected_len) {
 800                 sensor_info[s].report_ts = get_timestamp();
 801                 sensor_info[s].report_pending = 1;
 802         }
 803 }
 804
 805
 806 static int get_poll_wait_timeout (void)
 807 {
 808         /*
 809          * Compute an appropriate timeout value, in ms, for the epoll_wait
 810          * call that's going to await for iio device reports and incoming
 811          * reports from our sensor sysfs data reader threads.
 812          */
 813
 814         int s;
 815         int64_t target_ts = INT64_MAX;
 816         int64_t ms_to_wait;
 817         int64_t period;
 818
 819         /*
 820          * Check if have have to deal with "terse" drivers that only send events
 821          * when there is motion, despite the fact that the associated Android
 822          * sensor type is continuous rather than on-change. In that case we have
 823          * to duplicate events. Check deadline for the nearest upcoming event.
 824          */
 825         for (s=0; s<sensor_count; s++)
 826                 if (sensor_info[s].enable_count &&
 827                     (sensor_info[s].quirks & QUIRK_TERSE_DRIVER) &&
 828                     sensor_info[s].sampling_rate) {
 829                         period = (int64_t) (1000000000.0 /
 830                                                 sensor_info[s].sampling_rate);
 831
 832                         if (sensor_info[s].report_ts + period < target_ts)
 833                                 target_ts = sensor_info[s].report_ts + period;
 834                 }
 835
 836         /* If we don't have such a driver to deal with */
 837         if (target_ts == INT64_MAX)
 838                 return -1; /* Infinite wait */
 839
 840         ms_to_wait = (target_ts - get_timestamp()) / 1000000;
 841
 842         /* If the target timestamp is already behind us, don't wait */
 843         if (ms_to_wait < 1)
 844                 return 0;
 845
 846         return ms_to_wait;
 847 }
 848
 849
 850 int sensor_poll(struct sensors_event_t* data, int count)
 851 {
 852         int s;
 853         int i;
 854         int nfds;
 855         struct epoll_event ev[MAX_DEVICES];
 856         int returned_events;
 857         int event_count;
 858
 859         /* Get one or more events from our collection of sensors */
 860
 861 return_available_sensor_reports:
 862
 863         returned_events = 0;
 864
 865         /* Check our sensor collection for available reports */
 866         for (s=0; s<sensor_count && returned_events < count; s++)
 867                 if (sensor_info[s].report_pending) {
 868                         event_count = 0;
 869                         /* Lower flag */
 870                         sensor_info[s].report_pending = 0;
 871
 872                         /* Report this event if it looks OK */
 873                         event_count = propagate_sensor_report(s, &data[returned_events]);
 874
 875                         /* Duplicate only if both cal & uncal are active */
 876                         if (sensor_catalog[sensor_info[s].catalog_index].type == SENSOR_TYPE_GYROSCOPE &&
 877                                         sensor_info[s].pair_idx && sensor_info[sensor_info[s].pair_idx].enable_count != 0) {
 878                                         struct gyro_cal* gyro_data = (struct gyro_cal*) sensor_info[s].cal_data;
 879
 880                                         memcpy(&data[returned_events + event_count], &data[returned_events],
 881                                                         sizeof(struct sensors_event_t) * event_count);
 882                                         for (i = 0; i < event_count; i++) {
 883                                                 data[returned_events + i].type = SENSOR_TYPE_GYROSCOPE_UNCALIBRATED;
 884                                                 data[returned_events + i].sensor = sensor_info[s].pair_idx;
 885
 886                                                 data[returned_events + i].data[0] = data[returned_events + i].data[0] + gyro_data->bias[0];
 887                                                 data[returned_events + i].data[1] = data[returned_events + i].data[1] + gyro_data->bias[1];
 888                                                 data[returned_events + i].data[2] = data[returned_events + i].data[2] + gyro_data->bias[2];
 889
 890                                                 data[returned_events + i].uncalibrated_gyro.bias[0] = gyro_data->bias[0];
 891                                                 data[returned_events + i].uncalibrated_gyro.bias[1] = gyro_data->bias[1];
 892                                                 data[returned_events + i].uncalibrated_gyro.bias[2] = gyro_data->bias[2];
 893                                         }
 894                                         event_count <<= 1;
 895                         }
 896                         sensor_info[sensor_info[s].pair_idx].report_pending = 0;
 897                         returned_events += event_count;
 898                         /*
 899                          * If the sample was deemed invalid or unreportable,
 900                          * e.g. had the same value as the previously reported
 901                          * value for a 'on change' sensor, silently drop it.
 902                          */
 903                 }
 904
 905         if (returned_events)
 906                 return returned_events;
 907
 908 await_event:
 909
 910         ALOGV("Awaiting sensor data\n");
 911
 912         nfds = epoll_wait(poll_fd, ev, MAX_DEVICES, get_poll_wait_timeout());
 913
 914         if (nfds == -1) {
 915                 ALOGE("epoll_wait returned -1 (%s)\n", strerror(errno));
 916                 goto await_event;
 917         }
 918
 919         /* Synthetize duplicate samples if needed */
 920         synthetize_duplicate_samples();
 921
 922         ALOGV("%d fds signalled\n", nfds);
 923
 924         /* For each of the signalled sources */
 925         for (i=0; i<nfds; i++)
 926                 if (ev[i].events == EPOLLIN)
 927                         switch (ev[i].data.u32) {
 928                                 case 0 ... MAX_DEVICES-1:
 929                                         /* Read report from iio char dev fd */
 930                                         integrate_device_report(ev[i].data.u32);
 931                                         break;
 932
 933                                 case THREAD_REPORT_TAG_BASE ...
 934                                      THREAD_REPORT_TAG_BASE + MAX_SENSORS-1:
 935                                         /* Get report from acquisition thread */
 936                                         integrate_thread_report(ev[i].data.u32);
 937                                         break;
 938
 939                                 default:
 940                                         ALOGW("Unexpected event source!\n");
 941                                         break;
 942                         }
 943
 944         goto return_available_sensor_reports;
 945 }
 946
 947
 948 int sensor_set_delay(int s, int64_t ns)
 949 {
 950         /* Set the rate at which a specific sensor should report events */
 951
 952         /* See Android sensors.h for indication on sensor trigger modes */
 953
 954         char sysfs_path[PATH_MAX];
 955         char avail_sysfs_path[PATH_MAX];
 956         int dev_num             =       sensor_info[s].dev_num;
 957         int i                   =       sensor_info[s].catalog_index;
 958         const char *prefix      =       sensor_catalog[i].tag;
 959         float new_sampling_rate; /* Granted sampling rate after arbitration   */
 960         float cur_sampling_rate; /* Currently used sampling rate              */
 961         int per_sensor_sampling_rate;
 962         int per_device_sampling_rate;
 963         float max_supported_rate = 0;
 964         char freqs_buf[100];
 965         char* cursor;
 966         int n;
 967         float sr;
 968
 969         if (!ns) {
 970                 ALOGE("Rejecting zero delay request on sensor %d\n", s);
 971                 return -EINVAL;
 972         }
 973
 974         new_sampling_rate = 1000000000LL/ns;
 975
 976         /*
 977          * Artificially limit ourselves to 1 Hz or higher. This is mostly to
 978          * avoid setting up the stage for divisions by zero.
 979          */
 980         if (new_sampling_rate < 1)
 981                 new_sampling_rate = 1;
 982
 983         sensor_info[s].sampling_rate = new_sampling_rate;
 984
 985         /* If we're dealing with a poll-mode sensor */
 986         if (!sensor_info[s].num_channels) {
 987                 /* Interrupt current sleep so the new sampling gets used */
 988                 pthread_cond_signal(&thread_release_cond[s]);
 989                 return 0;
 990         }
 991
 992         sprintf(sysfs_path, SENSOR_SAMPLING_PATH, dev_num, prefix);
 993
 994         if (sysfs_read_float(sysfs_path, &cur_sampling_rate) != -1) {
 995                 per_sensor_sampling_rate = 1;
 996                 per_device_sampling_rate = 0;
 997         } else {
 998                 per_sensor_sampling_rate = 0;
 999
1000                 sprintf(sysfs_path, DEVICE_SAMPLING_PATH, dev_num);
1001
1002                 if (sysfs_read_float(sysfs_path, &cur_sampling_rate) != -1)
1003                         per_device_sampling_rate = 1;
1004                 else
1005                         per_device_sampling_rate = 0;
1006         }
1007
1008         if (!per_sensor_sampling_rate && !per_device_sampling_rate) {
1009                 ALOGE("No way to adjust sampling rate on sensor %d\n", s);
1010                 return -ENOSYS;
1011         }
1012
1013         /* Coordinate with others active sensors on the same device, if any */
1014         if (per_device_sampling_rate)
1015                 for (n=0; n<sensor_count; n++)
1016                         if (n != s && sensor_info[n].dev_num == dev_num &&
1017                             sensor_info[n].num_channels &&
1018                             sensor_info[n].enable_count &&
1019                             sensor_info[n].sampling_rate > new_sampling_rate)
1020                                 new_sampling_rate= sensor_info[n].sampling_rate;
1021
1022         /* Check if we have contraints on allowed sampling rates */
1023
1024         sprintf(avail_sysfs_path, DEVICE_AVAIL_FREQ_PATH, dev_num);
1025
1026         if (sysfs_read_str(avail_sysfs_path, freqs_buf, sizeof(freqs_buf)) > 0){
1027                 cursor = freqs_buf;
1028
1029                 /* Decode allowed sampling rates string, ex: "10 20 50 100" */
1030
1031                 /* While we're not at the end of the string */
1032                 while (*cursor && cursor[0]) {
1033
1034                         /* Decode a single value */
1035                         sr = strtod(cursor, NULL);
1036
1037                         if (sr > max_supported_rate)
1038                                 max_supported_rate = sr;
1039
1040                         /* If this matches the selected rate, we're happy */
1041                         if (new_sampling_rate == sr)
1042                                 break;
1043
1044                         /*
1045                          * If we reached a higher value than the desired rate,
1046                          * adjust selected rate so it matches the first higher
1047                          * available one and stop parsing - this makes the
1048                          * assumption that rates are sorted by increasing value
1049                          * in the allowed frequencies string.
1050                          */
1051                         if (sr > new_sampling_rate) {
1052                                 new_sampling_rate = sr;
1053                                 break;
1054                         }
1055
1056                         /* Skip digits */
1057                         while (cursor[0] && !isspace(cursor[0]))
1058                                 cursor++;
1059
1060                         /* Skip spaces */
1061                         while (cursor[0] && isspace(cursor[0]))
1062                                         cursor++;
1063                 }
1064         }
1065
1066
1067         if (max_supported_rate &&
1068                 new_sampling_rate > max_supported_rate) {
1069                 new_sampling_rate = max_supported_rate;
1070         }
1071
1072
1073         /* If the desired rate is already active we're all set */
1074         if (new_sampling_rate == cur_sampling_rate)
1075                 return 0;
1076
1077         ALOGI("Sensor %d sampling rate set to %g\n", s, new_sampling_rate);
1078
1079         if (trig_sensors_per_dev[dev_num])
1080                 enable_buffer(dev_num, 0);
1081
1082         sysfs_write_float(sysfs_path, new_sampling_rate);
1083
1084         if (trig_sensors_per_dev[dev_num])
1085                 enable_buffer(dev_num, 1);
1086
1087         return 0;
1088 }
1089
1090
1091 int allocate_control_data (void)
1092 {
1093         int i;
1094
1095         for (i=0; i<MAX_DEVICES; i++)
1096                 device_fd[i] = -1;
1097
1098         poll_fd = epoll_create(MAX_DEVICES);
1099
1100         if (poll_fd == -1) {
1101                 ALOGE("Can't create epoll instance for iio sensors!\n");
1102                 return -1;
1103         }
1104
1105         return poll_fd;
1106 }
1107
1108
1109 void delete_control_data (void)
1110 {
1111 }