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                 sensor_info[s].enable_count--;
 238
 239                 if (sensor_info[s].enable_count > 0)
 240                         return 0; /* The sensor was, and remains, in use */
 241
 242                 /* Sensor disabled, lower report available flag */
 243                 sensor_info[s].report_pending = 0;
 244
 245                 if (sensor_type == SENSOR_TYPE_MAGNETIC_FIELD)
 246                         compass_store_data(&sensor_info[s]);
 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 s)
 275 {
 276         int catalog_index = sensor_info[s].catalog_index;
 277         int sensor_type   = sensor_catalog[catalog_index].type;
 278
 279         switch (sensor_type) {
 280                 case SENSOR_TYPE_ACCELEROMETER:         /* m/s^2        */
 281                 case SENSOR_TYPE_MAGNETIC_FIELD:        /* micro-tesla  */
 282                 case SENSOR_TYPE_ORIENTATION:           /* degrees      */
 283                 case SENSOR_TYPE_GYROSCOPE:             /* radians/s    */
 284                         return 3;
 285
 286                 case SENSOR_TYPE_LIGHT:                 /* SI lux units */
 287                 case SENSOR_TYPE_AMBIENT_TEMPERATURE:   /* °C          */
 288                 case SENSOR_TYPE_TEMPERATURE:           /* °C          */
 289                 case SENSOR_TYPE_PROXIMITY:             /* centimeters  */
 290                 case SENSOR_TYPE_PRESSURE:              /* hecto-pascal */
 291                 case SENSOR_TYPE_RELATIVE_HUMIDITY:     /* percent */
 292                         return 1;
 293
 294                 case SENSOR_TYPE_ROTATION_VECTOR:
 295                         return  4;
 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 num_fields;
 326         uint32_t period;
 327         int64_t entry_ts;
 328         struct sensors_event_t data = {0};
 329         int c;
 330         int sampling_rate;
 331         int ret;
 332         uint32_t elapsed;
 333
 334         ALOGV("Entering data acquisition thread for sensor %d\n", s);
 335
 336         if (s < 0 || s >= sensor_count) {
 337                 ALOGE("Invalid sensor handle!\n");
 338                 return NULL;
 339         }
 340
 341         if (!sensor_info[s].sampling_rate) {
 342                 ALOGE("Zero rate in acquisition routine for sensor %d\n", s);
 343                 return NULL;
 344         }
 345
 346         num_fields = get_field_count(s);
 347
 348         while (1) {
 349                 CHECK_CANCEL(s)
 350
 351                 /* Pinpoint the moment we start sampling */
 352                 entry_ts = get_timestamp();
 353
 354                 ALOGV("Acquiring sample data for sensor %d through sysfs\n", s);
 355
 356                 /* Read values through sysfs */
 357                 for (c=0; c<num_fields; c++) {
 358                         data.data[c] = acquire_immediate_value(s, c);
 359
 360                         ALOGV("\tfield %d: %f\n", c, data.data[c]);
 361                         CHECK_CANCEL(s)
 362                 }
 363
 364                 /* If the sample looks good */
 365                 if (sensor_info[s].ops.finalize(s, &data)) {
 366
 367                         /* Pipe it for transmission to poll loop */
 368                         ret = write(    sensor_info[s].thread_data_fd[1],
 369                                         data.data,
 370                                         num_fields * sizeof(float));
 371                 }
 372
 373                 CHECK_CANCEL(s)
 374
 375                 /* Sleep a little, deducting read & write times */
 376                 elapsed = (get_timestamp() - entry_ts) / 1000;
 377
 378                 period = (uint32_t)
 379                          (1000000000LL / sensor_info[s].sampling_rate / 1000);
 380
 381                 if (period > elapsed)
 382                         usleep(period - elapsed);
 383         }
 384
 385         return NULL;
 386 }
 387
 388
 389 static void start_acquisition_thread (int s)
 390 {
 391         int incoming_data_fd;
 392         int ret;
 393
 394         struct epoll_event ev = {0};
 395
 396         ALOGV("Initializing acquisition context for sensor %d\n", s);
 397
 398         /* Create a pipe for inter thread communication */
 399         ret = pipe(sensor_info[s].thread_data_fd);
 400
 401         incoming_data_fd = sensor_info[s].thread_data_fd[0];
 402
 403         ev.events = EPOLLIN;
 404         ev.data.u32 = THREAD_REPORT_TAG_BASE + s;
 405
 406         /* Add incoming side of pipe to our poll set, with a suitable tag */
 407         ret = epoll_ctl(poll_fd, EPOLL_CTL_ADD, incoming_data_fd , &ev);
 408
 409         /* Create and start worker thread */
 410         ret = pthread_create(   &sensor_info[s].acquisition_thread,
 411                                 NULL,
 412                                 acquisition_routine,
 413                                 (void*) s);
 414 }
 415
 416
 417 static void stop_acquisition_thread (int s)
 418 {
 419         int incoming_data_fd = sensor_info[s].thread_data_fd[0];
 420         int outgoing_data_fd = sensor_info[s].thread_data_fd[1];
 421
 422         ALOGV("Tearing down acquisition context for sensor %d\n", s);
 423
 424         /* Delete the incoming side of the pipe from our poll set */
 425         epoll_ctl(poll_fd, EPOLL_CTL_DEL, incoming_data_fd, NULL);
 426
 427         /* Mark the pipe ends as invalid ; that's a cheap exit signal */
 428         sensor_info[s].thread_data_fd[0] = -1;
 429         sensor_info[s].thread_data_fd[1] = -1;
 430
 431         /* Close both sides of our pipe */
 432         close(incoming_data_fd);
 433         close(outgoing_data_fd);
 434
 435         /* Wait end of thread, and clean up thread handle */
 436         pthread_join(sensor_info[s].acquisition_thread, NULL);
 437
 438         /* Clean up our sensor descriptor */
 439         sensor_info[s].acquisition_thread = -1;
 440 }
 441
 442
 443 int sensor_activate(int s, int enabled)
 444 {
 445         char device_name[PATH_MAX];
 446         char trigger_name[MAX_NAME_SIZE + 16];
 447         int c;
 448         struct epoll_event ev = {0};
 449         int dev_fd;
 450         int ret;
 451         int dev_num = sensor_info[s].dev_num;
 452         int i = sensor_info[s].catalog_index;
 453         int is_poll_sensor = !sensor_info[s].num_channels;
 454
 455         ret = adjust_counters(s, enabled);
 456
 457         /* If the operation was neutral in terms of state, we're done */
 458         if (ret <= 0)
 459                 return ret;
 460
 461         if (!is_poll_sensor) {
 462
 463                 /* Stop sampling */
 464                 enable_buffer(dev_num, 0);
 465                 setup_trigger(dev_num, "\n");
 466
 467                 /* If there's at least one sensor enabled on this iio device */
 468                 if (trig_sensors_per_dev[dev_num]) {
 469                         sprintf(trigger_name, "%s-dev%d",
 470                                         sensor_info[s].internal_name, dev_num);
 471
 472                         /* Start sampling */
 473                         setup_trigger(dev_num, trigger_name);
 474                         enable_buffer(dev_num, 1);
 475                 }
 476         }
 477
 478         /*
 479          * Make sure we have a fd on the character device ; conversely, close
 480          * the fd if no one is using associated sensors anymore. The assumption
 481          * here is that the underlying driver will power on the relevant
 482          * hardware block while someone holds a fd on the device.
 483          */
 484         dev_fd = device_fd[dev_num];
 485
 486         if (!enabled) {
 487                 if (is_poll_sensor)
 488                         stop_acquisition_thread(s);
 489
 490                 if (dev_fd != -1 && !poll_sensors_per_dev[dev_num] &&
 491                         !trig_sensors_per_dev[dev_num]) {
 492                                 /*
 493                                  * Stop watching this fd. This should be a no-op
 494                                  * in case this fd was not in the poll set.
 495                                  */
 496                                 epoll_ctl(poll_fd, EPOLL_CTL_DEL, dev_fd, NULL);
 497
 498                                 close(dev_fd);
 499                                 device_fd[dev_num] = -1;
 500                         }
 501                 return 0;
 502         }
 503
 504         if (dev_fd == -1) {
 505                 /* First enabled sensor on this iio device */
 506                 sprintf(device_name, DEV_FILE_PATH, dev_num);
 507                 dev_fd = open(device_name, O_RDONLY | O_NONBLOCK);
 508
 509                 device_fd[dev_num] = dev_fd;
 510
 511                 if (dev_fd == -1) {
 512                         ALOGE("Could not open fd on %s (%s)\n",
 513                               device_name, strerror(errno));
 514                         adjust_counters(s, 0);
 515                         return -1;
 516                 }
 517
 518                 ALOGV("Opened %s: fd=%d\n", device_name, dev_fd);
 519
 520                 if (!is_poll_sensor) {
 521
 522                         /* Add this iio device fd to the set of watched fds */
 523                         ev.events = EPOLLIN;
 524                         ev.data.u32 = dev_num;
 525
 526                         ret = epoll_ctl(poll_fd, EPOLL_CTL_ADD, dev_fd, &ev);
 527
 528                         if (ret == -1) {
 529                                 ALOGE(  "Failed adding %d to poll set (%s)\n",
 530                                         dev_fd, strerror(errno));
 531                                 return -1;
 532                         }
 533
 534                         /* Note: poll-mode fds are not readable */
 535                 }
 536         }
 537
 538         /* Ensure that on-change sensors send at least one event after enable */
 539         sensor_info[s].prev_val = -1;
 540
 541         if (is_poll_sensor)
 542                 start_acquisition_thread(s);
 543
 544         return 0;
 545 }
 546
 547
 548 static int integrate_device_report(int dev_num)
 549 {
 550         int len;
 551         int s,c;
 552         unsigned char buf[MAX_SENSOR_REPORT_SIZE] = { 0 };
 553         int sr_offset;
 554         unsigned char *target;
 555         unsigned char *source;
 556         int size;
 557         int ts;
 558
 559         /* There's an incoming report on the specified iio device char dev fd */
 560
 561         if (dev_num < 0 || dev_num >= MAX_DEVICES) {
 562                 ALOGE("Event reported on unexpected iio device %d\n", dev_num);
 563                 return -1;
 564         }
 565
 566         if (device_fd[dev_num] == -1) {
 567                 ALOGE("Ignoring stale report on iio device %d\n", dev_num);
 568                 return -1;
 569         }
 570
 571         ts = get_timestamp();
 572
 573         len = read(device_fd[dev_num], buf, MAX_SENSOR_REPORT_SIZE);
 574
 575         if (len == -1) {
 576                 ALOGE("Could not read report from iio device %d (%s)\n",
 577                       dev_num, strerror(errno));
 578                 return -1;
 579         }
 580
 581         ALOGV("Read %d bytes from iio device %d\n", len, dev_num);
 582
 583         for (s=0; s<MAX_SENSORS; s++)
 584                 if (sensor_info[s].dev_num == dev_num &&
 585                     sensor_info[s].enable_count) {
 586
 587                         sr_offset = 0;
 588
 589                         /* Copy data from device to sensor report buffer */
 590                         for (c=0; c<sensor_info[s].num_channels; c++) {
 591
 592                                 target = sensor_info[s].report_buffer +
 593                                         sr_offset;
 594
 595                                 source = buf + sensor_info[s].channel[c].offset;
 596
 597                                 size = sensor_info[s].channel[c].size;
 598
 599                                 memcpy(target, source, size);
 600
 601                                 sr_offset += size;
 602                         }
 603
 604                         ALOGV("Sensor %d report available (%d bytes)\n", s,
 605                               sr_offset);
 606
 607                         sensor_info[s].report_ts = ts;
 608                         sensor_info[s].report_pending = 1;
 609                 }
 610
 611         return 0;
 612 }
 613
 614
 615 static int propagate_sensor_report(int s, struct sensors_event_t  *data)
 616 {
 617         /* There's a sensor report pending for this sensor ; transmit it */
 618
 619         int catalog_index = sensor_info[s].catalog_index;
 620         int sensor_type   = sensor_catalog[catalog_index].type;
 621         int num_fields    = get_field_count(s);
 622         int c;
 623         unsigned char* current_sample;
 624
 625         /* If there's nothing to return... we're done */
 626         if (!num_fields)
 627                 return 0;
 628
 629         memset(data, 0, sizeof(sensors_event_t));
 630
 631         data->version   = sizeof(sensors_event_t);
 632         data->sensor    = s;
 633         data->type      = sensor_type;
 634         data->timestamp = sensor_info[s].report_ts;
 635
 636         ALOGV("Sample on sensor %d (type %d):\n", s, sensor_type);
 637
 638         current_sample = sensor_info[s].report_buffer;
 639
 640         /* If this is a poll sensor */
 641         if (!sensor_info[s].num_channels) {
 642                 /* Use the data provided by the acquisition thread */
 643                 ALOGV("Reporting data from worker thread for S%d\n", s);
 644                 memcpy(data->data, current_sample, num_fields * sizeof(float));
 645                 return 1;
 646         }
 647
 648         /* Convert the data into the expected Android-level format */
 649         for (c=0; c<num_fields; c++) {
 650
 651                 data->data[c] = sensor_info[s].ops.transform
 652                                                         (s, c, current_sample);
 653
 654                 ALOGV("\tfield %d: %f\n", c, data->data[c]);
 655                 current_sample += sensor_info[s].channel[c].size;
 656         }
 657
 658         /*
 659          * The finalize routine, in addition to its late sample processing duty,
 660          * has the final say on whether or not the sample gets sent to Android.
 661          */
 662         return sensor_info[s].ops.finalize(s, data);
 663 }
 664
 665
 666 static void integrate_thread_report (uint32_t tag)
 667 {
 668         int s = tag - THREAD_REPORT_TAG_BASE;
 669         int len;
 670         int expected_len;
 671
 672         expected_len = get_field_count(s) * sizeof(float);
 673
 674         len = read(sensor_info[s].thread_data_fd[0],
 675                    sensor_info[s].report_buffer,
 676                    expected_len);
 677
 678         if (len == expected_len) {
 679                 sensor_info[s].report_ts = get_timestamp();
 680                 sensor_info[s].report_pending = 1;
 681         }
 682 }
 683
 684
 685 int sensor_poll(struct sensors_event_t* data, int count)
 686 {
 687         int s;
 688         int i;
 689         int nfds;
 690         struct epoll_event ev[MAX_DEVICES];
 691         int64_t target_ts;
 692         int returned_events;
 693
 694         /* Get one or more events from our collection of sensors */
 695
 696 return_available_sensor_reports:
 697
 698         returned_events = 0;
 699
 700         /* Check our sensor collection for available reports */
 701         for (s=0; s<sensor_count && returned_events<count; s++)
 702                 if (sensor_info[s].report_pending) {
 703
 704                         /* Lower flag */
 705                         sensor_info[s].report_pending = 0;
 706
 707                         /* Report this event if it looks OK */
 708                         returned_events +=
 709                              propagate_sensor_report(s, &data[returned_events]);
 710
 711                         /*
 712                          * If the sample was deemed invalid or unreportable,
 713                          * e.g. had the same value as the previously reported
 714                          * value for a 'on change' sensor, silently drop it.
 715                          */
 716                 }
 717
 718         if (returned_events)
 719                 return returned_events;
 720
 721 await_event:
 722
 723         ALOGV("Awaiting sensor data\n");
 724
 725         nfds = epoll_wait(poll_fd, ev, MAX_DEVICES, -1);
 726
 727         if (nfds == -1) {
 728                 ALOGI("epoll_wait returned -1 (%s)\n", strerror(errno));
 729                 goto await_event;
 730         }
 731
 732         ALOGV("%d fds signalled\n", nfds);
 733
 734         /* For each of the signalled sources */
 735         for (i=0; i<nfds; i++)
 736                 if (ev[i].events == EPOLLIN)
 737                         switch (ev[i].data.u32) {
 738                                 case 0 ... MAX_DEVICES-1:
 739                                         /* Read report from iio char dev fd */
 740                                         integrate_device_report(ev[i].data.u32);
 741                                         break;
 742
 743                                 case THREAD_REPORT_TAG_BASE ...
 744                                      THREAD_REPORT_TAG_BASE + MAX_SENSORS-1:
 745                                         /* Get report from acquisition thread */
 746                                         integrate_thread_report(ev[i].data.u32);
 747                                         break;
 748
 749                                 default:
 750                                         ALOGW("Unexpected event source!\n");
 751                                         break;
 752                         }
 753
 754         goto return_available_sensor_reports;
 755 }
 756
 757
 758 int sensor_set_delay(int s, int64_t ns)
 759 {
 760         /* Set the rate at which a specific sensor should report events */
 761
 762         /* See Android sensors.h for indication on sensor trigger modes */
 763
 764         char sysfs_path[PATH_MAX];
 765         char avail_sysfs_path[PATH_MAX];
 766         int dev_num             =       sensor_info[s].dev_num;
 767         int i                   =       sensor_info[s].catalog_index;
 768         const char *prefix      =       sensor_catalog[i].tag;
 769         float new_sampling_rate; /* Granted sampling rate after arbitration   */
 770         float cur_sampling_rate; /* Currently used sampling rate              */
 771         int per_sensor_sampling_rate;
 772         int per_device_sampling_rate;
 773         float max_supported_rate = 0;
 774         char freqs_buf[100];
 775         char* cursor;
 776         int n;
 777         float sr;
 778
 779         if (!ns) {
 780                 ALOGE("Rejecting zero delay request on sensor %d\n", s);
 781                 return -EINVAL;
 782         }
 783
 784         new_sampling_rate = 1000000000LL/ns;
 785
 786         /*
 787          * Artificially limit ourselves to 1 Hz or higher. This is mostly to
 788          * avoid setting up the stage for divisions by zero.
 789          */
 790         if (new_sampling_rate < 1)
 791                 new_sampling_rate = 1;
 792
 793         sensor_info[s].sampling_rate = new_sampling_rate;
 794
 795         /* If we're dealing with a poll-mode sensor */
 796         if (!sensor_info[s].num_channels) {
 797                 /* The new sampling rate will be used on next iteration */
 798                 return 0;
 799         }
 800
 801         sprintf(sysfs_path, SENSOR_SAMPLING_PATH, dev_num, prefix);
 802
 803         if (sysfs_read_float(sysfs_path, &cur_sampling_rate) != -1) {
 804                 per_sensor_sampling_rate = 1;
 805                 per_device_sampling_rate = 0;
 806         } else {
 807                 per_sensor_sampling_rate = 0;
 808
 809                 sprintf(sysfs_path, DEVICE_SAMPLING_PATH, dev_num);
 810
 811                 if (sysfs_read_float(sysfs_path, &cur_sampling_rate) != -1)
 812                         per_device_sampling_rate = 1;
 813                 else
 814                         per_device_sampling_rate = 0;
 815         }
 816
 817         if (!per_sensor_sampling_rate && !per_device_sampling_rate) {
 818                 ALOGE("No way to adjust sampling rate on sensor %d\n", s);
 819                 return -ENOSYS;
 820         }
 821
 822         /* Coordinate with others active sensors on the same device, if any */
 823         if (per_device_sampling_rate)
 824                 for (n=0; n<sensor_count; n++)
 825                         if (n != s && sensor_info[n].dev_num == dev_num &&
 826                             sensor_info[n].num_channels &&
 827                             sensor_info[n].enable_count &&
 828                             sensor_info[n].sampling_rate > new_sampling_rate)
 829                                 new_sampling_rate= sensor_info[n].sampling_rate;
 830
 831         /* Check if we have contraints on allowed sampling rates */
 832
 833         sprintf(avail_sysfs_path, DEVICE_AVAIL_FREQ_PATH, dev_num);
 834
 835         if (sysfs_read_str(avail_sysfs_path, freqs_buf, sizeof(freqs_buf)) > 0){
 836                 cursor = freqs_buf;
 837
 838                 /* Decode allowed sampling rates string, ex: "10 20 50 100" */
 839
 840                 /* While we're not at the end of the string */
 841                 while (*cursor && cursor[0]) {
 842
 843                         /* Decode a single value */
 844                         sr = strtod(cursor, NULL);
 845
 846                         if (sr > max_supported_rate)
 847                                 max_supported_rate = sr;
 848
 849                         /* If this matches the selected rate, we're happy */
 850                         if (new_sampling_rate == sr)
 851                                 break;
 852
 853                         /*
 854                          * If we reached a higher value than the desired rate,
 855                          * adjust selected rate so it matches the first higher
 856                          * available one and stop parsing - this makes the
 857                          * assumption that rates are sorted by increasing value
 858                          * in the allowed frequencies string.
 859                          */
 860                         if (sr > new_sampling_rate) {
 861                                 new_sampling_rate = sr;
 862                                 break;
 863                         }
 864
 865                         /* Skip digits */
 866                         while (cursor[0] && !isspace(cursor[0]))
 867                                 cursor++;
 868
 869                         /* Skip spaces */
 870                         while (cursor[0] && isspace(cursor[0]))
 871                                         cursor++;
 872                 }
 873         }
 874
 875
 876         if (max_supported_rate &&
 877                 new_sampling_rate > max_supported_rate) {
 878                 new_sampling_rate = max_supported_rate;
 879         }
 880
 881
 882         /* If the desired rate is already active we're all set */
 883         if (new_sampling_rate == cur_sampling_rate)
 884                 return 0;
 885
 886         ALOGI("Sensor %d sampling rate set to %g\n", s, new_sampling_rate);
 887
 888         if (trig_sensors_per_dev[dev_num])
 889                 enable_buffer(dev_num, 0);
 890
 891         sysfs_write_float(sysfs_path, new_sampling_rate);
 892
 893         if (trig_sensors_per_dev[dev_num])
 894                 enable_buffer(dev_num, 1);
 895
 896         return 0;
 897 }
 898
 899
 900 int allocate_control_data (void)
 901 {
 902         int i;
 903         struct epoll_event ev = {0};
 904
 905         for (i=0; i<MAX_DEVICES; i++)
 906                 device_fd[i] = -1;
 907
 908         poll_fd = epoll_create(MAX_DEVICES);
 909
 910         if (poll_fd == -1) {
 911                 ALOGE("Can't create epoll instance for iio sensors!\n");
 912                 return -1;
 913         }
 914
 915         return poll_fd;
 916 }
 917
 918
 919 void delete_control_data (void)
 920 {
 921 }