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