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