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 <sys/epoll.h>
   9 #include <sys/socket.h>
  10 #include <utils/Log.h>
  11 #include <hardware/sensors.h>
  12 #include "control.h"
  13 #include "enumeration.h"
  14 #include "utils.h"
  15 #include "transform.h"
  16 #include "calibration.h"
  17
  18 /* Currently active sensors count, per device */
  19 static int poll_sensors_per_dev[MAX_DEVICES];   /* poll-mode sensors */
  20 static int trig_sensors_per_dev[MAX_DEVICES];   /* trigger, event based */
  21
  22 static int device_fd[MAX_DEVICES];   /* fd on the /dev/iio:deviceX file */
  23
  24 static int poll_fd; /* epoll instance covering all enabled sensors */
  25
  26 static int poll_socket_pair[2]; /* used to unblock the poll loop */
  27
  28 /* Timestamp for the moment when we last exited a poll operation */
  29 static int64_t last_poll_exit_ts;
  30
  31 static int active_poll_sensors; /* Number of enabled poll-mode sensors */
  32
  33 /* Cap the time between poll operations to this, to counter runaway polls */
  34 #define POLL_MIN_INTERVAL 1000 /* uS */
  35
  36 #define INVALID_DEV_NUM ((uint32_t) -1)
  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 static void refresh_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_enabled;
  81         int ch_index;
  82         char* ch_spec;
  83         char spec_buf[MAX_TYPE_SPEC_LEN];
  84         struct datum_info_t* ch_info;
  85         int size;
  86         char sysfs_path[PATH_MAX];
  87         int active_channels;
  88         int offset;
  89         int channel_size_from_index[MAX_SENSORS * MAX_CHANNELS] = { 0 };
  90         int sensor_handle_from_index[MAX_SENSORS * MAX_CHANNELS] = { 0 };
  91         int channel_number_from_index[MAX_SENSORS * MAX_CHANNELS] = { 0 };
  92
  93         active_channels = 0;
  94
  95         /* For each sensor that is linked to this device */
  96         for (s=0; s<sensor_count; s++) {
  97                 if (sensor_info[s].dev_num != dev_num)
  98                         continue;
  99
 100                 i = sensor_info[s].catalog_index;
 101
 102                 /* Read channel status through syfs attributes */
 103                 for (c=0; c<sensor_info[s].num_channels; c++) {
 104
 105                         /* Read _en file */
 106                         sprintf(sysfs_path, CHANNEL_PATH "%s",
 107                                 sensor_info[s].dev_num,
 108                                 sensor_catalog[i].channel[c].en_path);
 109
 110                         n = sysfs_read_int(sysfs_path, &ch_enabled);
 111
 112                         if (n == -1) {
 113                                 ALOGW(  "Failed to read _en flag: %s\n",
 114                                 sysfs_path);
 115                                 continue;
 116                         }
 117
 118                         if (!ch_enabled != 1) {
 119                                 sensor_info[s].channel[c].size = 0;
 120                         }
 121
 122                         /* Read _type file */
 123                         sprintf(sysfs_path, CHANNEL_PATH "%s",
 124                                 sensor_info[s].dev_num,
 125                                 sensor_catalog[i].channel[c].type_path);
 126
 127                         n = sysfs_read_str(sysfs_path, spec_buf,
 128                                                 sizeof(spec_buf));
 129
 130                         if (n == -1) {
 131                                         ALOGW(  "Failed to read type: %s\n",
 132                                         sysfs_path);
 133                                         continue;
 134                                 }
 135
 136                         ch_spec = sensor_info[s].channel[c].type_spec;
 137
 138                         memcpy(ch_spec, spec_buf, sizeof(spec_buf));
 139
 140                         ch_info = &sensor_info[s].channel[c].type_info;
 141
 142                         size = decode_type_spec(ch_spec, ch_info);
 143
 144                         /* Read _index file */
 145                         sprintf(sysfs_path, CHANNEL_PATH "%s",
 146                                 sensor_info[s].dev_num,
 147                                 sensor_catalog[i].channel[c].index_path);
 148
 149                         n = sysfs_read_int(sysfs_path, &ch_index);
 150
 151                         if (n == -1) {
 152                                         ALOGW(  "Failed to read index: %s\n",
 153                                                 sysfs_path);
 154                                         continue;
 155                                 }
 156
 157                         if (ch_index >= MAX_SENSORS) {
 158                                 ALOGE("Index out of bounds!: %s\n", sysfs_path);
 159                                 continue;
 160                         }
 161
 162                         /* Record what this index is about */
 163
 164                         sensor_handle_from_index [ch_index] = s;
 165                         channel_number_from_index[ch_index] = c;
 166                         channel_size_from_index  [ch_index] = size;
 167
 168                         active_channels++;
 169                 }
 170         }
 171
 172         ALOGI("Found %d enabled channels for iio device %d\n", active_channels,
 173                 dev_num);
 174
 175         /*
 176          * Now that we know which channels are enabled, their sizes and their
 177          * ordering, update channels offsets within device report. Note: there
 178          * is a possibility that several sensors share the same index, with
 179          * their data fields being isolated by masking and shifting as specified
 180          * through the real bits and shift values in type attributes. This case
 181          * is not currently supported. Also, the code below assumes no hole in
 182          * the sequence of indices, so it is dependent on discovery of all
 183          * sensors.
 184          */
 185          offset = 0;
 186          for (i=0; i<MAX_SENSORS * MAX_CHANNELS; i++) {
 187                 s =     sensor_handle_from_index[i];
 188                 c =     channel_number_from_index[i];
 189                 size =  channel_size_from_index[i];
 190
 191                 if (!size)
 192                         continue;
 193
 194                 ALOGI("S%d C%d : offset %d, size %d, type %s\n",
 195                       s, c, offset, size, sensor_info[s].channel[c].type_spec);
 196
 197                 sensor_info[s].channel[c].offset        = offset;
 198                 sensor_info[s].channel[c].size          = size;
 199
 200                 offset += size;
 201          }
 202 }
 203
 204
 205 int adjust_counters (int s, int enabled)
 206 {
 207         /*
 208          * Adjust counters based on sensor enable action. Return values are:
 209          * -1 if there's an inconsistency: abort action in this case
 210          *  0 if the operation was completed and we're all set
 211          *  1 if we toggled the state of the sensor and there's work left
 212          */
 213
 214         int dev_num = sensor_info[s].dev_num;
 215         int catalog_index = sensor_info[s].catalog_index;
 216         int sensor_type = sensor_catalog[catalog_index].type;
 217
 218         /* Refcount per sensor, in terms of enable count */
 219         if (enabled) {
 220                 ALOGI("Enabling sensor %d (iio device %d: %s)\n",
 221                         s, dev_num, sensor_info[s].friendly_name);
 222
 223                 sensor_info[s].enable_count++;
 224
 225                 if (sensor_info[s].enable_count != 1) {
 226                         return 0; /* The sensor was, and remains, in use */
 227                 } else {
 228                         if (sensor_type == SENSOR_TYPE_MAGNETIC_FIELD)
 229                                 compass_read_data(COMPASS_CALIBRATION_PATH);
 230                 }
 231         } else {
 232                 if (sensor_info[s].enable_count == 0)
 233                         return -1; /* Spurious disable call */
 234
 235                 ALOGI("Disabling sensor %d (iio device %d: %s)\n", s, dev_num,
 236                       sensor_info[s].friendly_name);
 237
 238                 if (sensor_type == SENSOR_TYPE_MAGNETIC_FIELD)
 239                         compass_store_data(COMPASS_CALIBRATION_PATH);
 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
 250         /* We changed the state of a sensor - adjust per iio device counters */
 251
 252         /* If this is a regular event-driven sensor */
 253         if (sensor_info[s].num_channels) {
 254
 255                         if (enabled)
 256                                 trig_sensors_per_dev[dev_num]++;
 257                         else
 258                                 trig_sensors_per_dev[dev_num]--;
 259
 260                         return 1;
 261                 }
 262
 263         if (enabled) {
 264                 active_poll_sensors++;
 265                 poll_sensors_per_dev[dev_num]++;
 266                 return 1;
 267         }
 268
 269         active_poll_sensors--;
 270         poll_sensors_per_dev[dev_num]--;
 271         return 1;
 272 }
 273
 274
 275 int sensor_activate(int s, int enabled)
 276 {
 277         char sysfs_path[PATH_MAX];
 278         char device_name[PATH_MAX];
 279         char trigger_name[MAX_NAME_SIZE + 16];
 280         int c;
 281         struct epoll_event ev = {0};
 282         int dev_fd;
 283         int ret;
 284         int dev_num = sensor_info[s].dev_num;
 285         int i = sensor_info[s].catalog_index;
 286         int is_poll_sensor = !sensor_info[s].num_channels;
 287
 288         ret = adjust_counters(s, enabled);
 289
 290         /* If the operation was neutral in terms of state, we're done */
 291         if (ret <= 0)
 292                 return ret;
 293
 294         if (!is_poll_sensor) {
 295                 /* Changes have to be made while the buffer is turned off */
 296                 enable_buffer(dev_num, 0);
 297
 298                 /* Configure trigger */
 299                 switch (trig_sensors_per_dev[dev_num]) {
 300                         case 0:
 301                                 setup_trigger(dev_num, "none");
 302                                 break;
 303
 304                         case 1:
 305                                 sprintf(trigger_name, "%s-dev%d",
 306                                         sensor_info[s].internal_name, dev_num);
 307
 308                                 setup_trigger(dev_num, trigger_name);
 309                                 break;
 310
 311                         default:
 312                                 /* The trigger is already set */
 313                                 break;
 314                 }
 315
 316                 /*
 317                  * Turn channels associated to this sensor on or off, and update
 318                  * the channels maps for all sensors associated to this device.
 319                  */
 320                 for (c=0;c<sensor_info[s].num_channels; c++) {
 321                         sprintf(sysfs_path, CHANNEL_PATH "%s",
 322                                 sensor_info[s].dev_num,
 323                                 sensor_catalog[i].channel[c].en_path);
 324
 325                         sysfs_write_int(sysfs_path, enabled);
 326                 }
 327
 328                 /* If there's at least one sensor left */
 329                 if (trig_sensors_per_dev[dev_num]) {
 330                         refresh_sensor_report_maps(dev_num);
 331                         enable_buffer(dev_num, 1);
 332                 }
 333         }
 334
 335         /*
 336          * Make sure we have a fd on the character device ; conversely, close
 337          * the fd if no one is using associated sensor anymore. The assumption
 338          * here is that the underlying driver will power on the relevant
 339          * hardware block while someone hold a fd on the device.
 340          */
 341         dev_fd = device_fd[dev_num];
 342
 343         if (!enabled) {
 344                 if (dev_fd != -1 && !poll_sensors_per_dev[dev_num] &&
 345                         !trig_sensors_per_dev[dev_num]) {
 346                                 /*
 347                                  * Stop watching this fd. This should be a no-op
 348                                  * in case this fd was not in the poll set.
 349                                  */
 350                                 epoll_ctl(poll_fd, EPOLL_CTL_DEL, dev_fd, NULL);
 351
 352                                 close(dev_fd);
 353                                 device_fd[dev_num] = -1;
 354                         }
 355                 return 0;
 356         }
 357
 358         if (dev_fd == -1) {
 359                 /* First enabled sensor on this iio device */
 360                 sprintf(device_name, DEV_FILE_PATH, dev_num);
 361                 dev_fd = open(device_name, O_RDONLY | O_NONBLOCK);
 362
 363                 device_fd[dev_num] = dev_fd;
 364
 365                 if (dev_fd == -1) {
 366                         ALOGE("Could not open fd on %s (%s)\n",
 367                               device_name, strerror(errno));
 368                         adjust_counters(s, 0);
 369                         return -1;
 370                 }
 371
 372                 ALOGV("Opened %s: fd=%d\n", device_name, dev_fd);
 373
 374                 if (!is_poll_sensor) {
 375
 376                         /* Add this iio device fd to the set of watched fds */
 377                         ev.events = EPOLLIN;
 378                         ev.data.u32 = dev_num;
 379
 380                         ret = epoll_ctl(poll_fd, EPOLL_CTL_ADD, dev_fd, &ev);
 381
 382                         if (ret == -1) {
 383                                 ALOGE(  "Failed adding %d to poll set (%s)\n",
 384                                         dev_fd, strerror(errno));
 385                                 return -1;
 386                         }
 387
 388                         /* Note: poll-mode fds are not readable */
 389                 }
 390         }
 391
 392         /* Release the polling loop so an updated timeout gets used */
 393         write(poll_socket_pair[1], "", 1);
 394
 395         return 0;
 396 }
 397
 398
 399 static int integrate_device_report(int dev_num)
 400 {
 401         int len;
 402         int s,c;
 403         unsigned char buf[MAX_SENSOR_REPORT_SIZE * MAX_SENSORS] = { 0 };
 404         int sr_offset;
 405         unsigned char *target;
 406         unsigned char *source;
 407         int size;
 408         int expected_size = 0;
 409
 410         /* There's an incoming report on the specified fd */
 411
 412         if (dev_num < 0 || dev_num >= MAX_DEVICES) {
 413                 ALOGE("Event reported on unexpected iio device %d\n", dev_num);
 414                 return -1;
 415         }
 416
 417         if (device_fd[dev_num] == -1) {
 418                 ALOGE("Ignoring stale report on iio device %d\n", dev_num);
 419                 return -1;
 420         }
 421
 422         for (s=0; s<MAX_SENSORS; s++)
 423                 if (sensor_info[s].dev_num == dev_num)
 424                         for (c=0; c<sensor_info[s].num_channels; c++)
 425                                 expected_size += sensor_info[s].channel[c].size;
 426
 427         len = read(device_fd[dev_num], buf, expected_size);
 428
 429         if (len == -1) {
 430                 ALOGE("Could not read report from iio device %d (%s)\n",
 431                       dev_num, strerror(errno));
 432                 return -1;
 433         }
 434
 435         ALOGV("Read %d bytes from iio device %d\n", len, dev_num);
 436
 437         for (s=0; s<MAX_SENSORS; s++)
 438                 if (sensor_info[s].dev_num == dev_num &&
 439                     sensor_info[s].enable_count) {
 440
 441                         sr_offset = 0;
 442
 443                         /* Copy data from device to sensor report buffer */
 444                         for (c=0; c<sensor_info[s].num_channels; c++) {
 445
 446                                 target = sensor_info[s].report_buffer +
 447                                         sr_offset;
 448
 449                                 source = buf + sensor_info[s].channel[c].offset;
 450
 451                                 size = sensor_info[s].channel[c].size;
 452
 453                                 memcpy(target, source, size);
 454
 455                                 sr_offset += size;
 456                         }
 457
 458                         ALOGV("Sensor %d report available (%d bytes)\n", s,
 459                               sr_offset);
 460
 461                         sensor_info[s].report_pending = 1;
 462                 }
 463
 464         return 0;
 465 }
 466
 467
 468 static void propagate_sensor_report(int s, struct sensors_event_t* data)
 469 {
 470         /* There's a sensor report pending for this sensor ; transmit it */
 471
 472         int catalog_index = sensor_info[s].catalog_index;
 473         int sensor_type = sensor_catalog[catalog_index].type;
 474         int num_fields;
 475         int c;
 476         unsigned char* current_sample;
 477         int64_t current_ts = get_timestamp();
 478
 479         memset(data, 0, sizeof(sensors_event_t));
 480
 481         data->version = sizeof(sensors_event_t);
 482         data->sensor = s;
 483         data->type = sensor_type;
 484         data->timestamp = current_ts;
 485
 486         switch (sensor_type) {
 487                 case SENSOR_TYPE_ACCELEROMETER:         /* m/s^2        */
 488                 case SENSOR_TYPE_MAGNETIC_FIELD:        /* micro-tesla  */
 489                 case SENSOR_TYPE_ORIENTATION:           /* degrees      */
 490                 case SENSOR_TYPE_GYROSCOPE:             /* radians/s    */
 491                         num_fields = 3;
 492                         break;
 493
 494                 case SENSOR_TYPE_LIGHT:                 /* SI lux units */
 495                 case SENSOR_TYPE_AMBIENT_TEMPERATURE:   /* °C          */
 496                 case SENSOR_TYPE_TEMPERATURE:           /* °C          */
 497                 case SENSOR_TYPE_PROXIMITY:             /* centimeters  */
 498                 case SENSOR_TYPE_PRESSURE:              /* hecto-pascal */
 499                 case SENSOR_TYPE_RELATIVE_HUMIDITY:     /* percent */
 500                         num_fields = 1;
 501                         break;
 502
 503                 case SENSOR_TYPE_ROTATION_VECTOR:
 504                         num_fields = 4;
 505                         break;
 506
 507                 case SENSOR_TYPE_DEVICE_PRIVATE_BASE:   /* hidden for now */
 508                         num_fields = 0;
 509                         break;
 510
 511                 default:
 512                         ALOGE("Unknown sensor type!\n");
 513                         num_fields = 0;
 514                         break;
 515         }
 516
 517         ALOGV("Sample on sensor %d (type %d):\n", s, sensor_type);
 518
 519         /* Take note of current time counter value for rate control purposes */
 520         sensor_info[s].last_integration_ts = current_ts;
 521
 522         /* If we're dealing with a poll-mode sensor */
 523         if (!sensor_info[s].num_channels) {
 524
 525                 /* Read values through sysfs rather than from a report buffer */
 526                 for (c=0; c<num_fields; c++) {
 527
 528                         data->data[c] = acquire_immediate_value(s, c);
 529
 530                         ALOGV("\tfield %d: %f\n", c, data->data[c]);
 531                 }
 532
 533                 sensor_info[s].ops.finalize(s, data);
 534                 return;
 535         }
 536
 537         /* Convert the data into the expected Android-level format */
 538
 539         current_sample = sensor_info[s].report_buffer;
 540
 541         for (c=0; c<num_fields; c++) {
 542
 543                 data->data[c] = sensor_info[s].ops.transform
 544                                                         (s, c, current_sample);
 545
 546                 ALOGV("\tfield %d: %f\n", c, data->data[c]);
 547                 current_sample += sensor_info[s].channel[c].size;
 548         }
 549
 550         sensor_info[s].ops.finalize(s, data);
 551 }
 552
 553
 554 static int get_poll_time (void)
 555 {
 556         int64_t target_ts;
 557         int64_t lowest_target_ts;
 558         int64_t current_ts;
 559         int s;
 560
 561         if (!active_poll_sensors)
 562                 return -1;      /* Infinite wait */
 563
 564         /* Check if we should schedule a poll-mode sensor event delivery */
 565
 566         lowest_target_ts = INT64_MAX;
 567
 568         for (s=0; s<sensor_count; s++)
 569                 if (sensor_info[s].enable_count &&
 570                     sensor_info[s].sampling_rate &&
 571                     !sensor_info[s].num_channels) {
 572                                 target_ts = sensor_info[s].last_integration_ts +
 573                                       1000000000LL/sensor_info[s].sampling_rate;
 574
 575                                 if (target_ts < lowest_target_ts)
 576                                         lowest_target_ts = target_ts;
 577                         }
 578
 579         if (lowest_target_ts == INT64_MAX)
 580                 return -1;
 581
 582         current_ts = get_timestamp();
 583
 584         if (lowest_target_ts <= current_ts)
 585                 return 0;
 586
 587         return (lowest_target_ts - current_ts)/1000000; /* ms */
 588 }
 589
 590
 591 static void acknowledge_release (void)
 592 {
 593         /* A write to our socket circuit was performed to release epoll */
 594         char buf;
 595         read(poll_socket_pair[0], &buf, 1);
 596 }
 597
 598
 599 int sensor_poll(struct sensors_event_t* data, int count)
 600 {
 601         int s;
 602         int i;
 603         int nfds;
 604         int delta;
 605         struct epoll_event ev[MAX_DEVICES];
 606         int64_t target_ts;
 607
 608         /* Get one or more events from our collection of sensors */
 609
 610 return_first_available_sensor_report:
 611
 612         /* If there's at least one available report */
 613         for (s=0; s<sensor_count; s++)
 614                 if (sensor_info[s].report_pending) {
 615
 616                         /* Return that up */
 617                         propagate_sensor_report(s, data);
 618                         sensor_info[s].report_pending = 0;
 619                         ALOGV("Report on sensor %d\n", s);
 620                         return 1;
 621                 }
 622 await_event:
 623
 624         /* Keep a minimum time interval between poll operations */
 625         delta = (get_timestamp() - last_poll_exit_ts)/1000;
 626
 627         if (delta > 0 && delta < POLL_MIN_INTERVAL)
 628                 usleep(POLL_MIN_INTERVAL - delta);
 629
 630         ALOGV("Awaiting sensor data\n");
 631
 632         nfds = epoll_wait(poll_fd, ev, MAX_DEVICES, get_poll_time());
 633
 634         last_poll_exit_ts = get_timestamp();
 635
 636         if (nfds == -1) {
 637                 ALOGI("epoll_wait returned -1 (%s)\n", strerror(errno));
 638                 goto await_event;
 639         }
 640
 641         ALOGV("%d fds signalled\n", nfds);
 642
 643         /* For each of the devices for which a report is available */
 644         for (i=0; i<nfds; i++)
 645                 if (ev[i].events == EPOLLIN) {
 646                         if (ev[i].data.u32 == INVALID_DEV_NUM) {
 647                                 acknowledge_release();
 648                                 goto await_event;
 649                         } else
 650                                 /* Read report */
 651                                 integrate_device_report(ev[i].data.u32);
 652                 }
 653
 654         /* Check poll-mode sensors and fire up an event if it's time to do so */
 655         if (active_poll_sensors)
 656                 for (s=0; s<sensor_count; s++)
 657                         if (sensor_info[s].enable_count &&
 658                             !sensor_info[s].num_channels &&
 659                             sensor_info[s].sampling_rate) {
 660                                 target_ts = sensor_info[s].last_integration_ts +
 661                                       1000000000LL/sensor_info[s].sampling_rate;
 662
 663                                 if (last_poll_exit_ts >= target_ts)
 664                                         sensor_info[s].report_pending = 1;
 665                         }
 666
 667         goto return_first_available_sensor_report;
 668 }
 669
 670
 671 int sensor_set_delay(int s, int64_t ns)
 672 {
 673         /* Set the rate at which a specific sensor should report events */
 674
 675         /* See Android sensors.h for indication on sensor trigger modes */
 676
 677         char sysfs_path[PATH_MAX];
 678         char avail_sysfs_path[PATH_MAX];
 679         int dev_num             =       sensor_info[s].dev_num;
 680         int i                   =       sensor_info[s].catalog_index;
 681         const char *prefix      =       sensor_catalog[i].tag;
 682         int new_sampling_rate;  /* Granted sampling rate after arbitration   */
 683         int cur_sampling_rate;  /* Currently used sampling rate              */
 684         int req_sampling_rate;  /* Requested ; may be different from granted */
 685         int per_sensor_sampling_rate;
 686         int per_device_sampling_rate;
 687         int max_supported_rate = 0;
 688         int limit;
 689         char freqs_buf[100];
 690         char* cursor;
 691         int n;
 692
 693         if (!ns) {
 694                 ALOGE("Rejecting zero delay request on sensor %d\n", s);
 695                 return -EINVAL;
 696         }
 697
 698         new_sampling_rate = req_sampling_rate = (int) (1000000000L/ns);
 699
 700         if (!new_sampling_rate) {
 701                 ALOGI("Sub-HZ sampling rate requested on on sensor %d\n", s);
 702                 new_sampling_rate = 1;
 703         }
 704
 705         sensor_info[s].sampling_rate = new_sampling_rate;
 706
 707         /* If we're dealing with a poll-mode sensor, release poll and return */
 708         if (!sensor_info[s].num_channels)
 709                 goto exit;
 710
 711         sprintf(sysfs_path, SENSOR_SAMPLING_PATH, dev_num, prefix);
 712
 713         if (sysfs_read_int(sysfs_path, &cur_sampling_rate) != -1) {
 714                 per_sensor_sampling_rate = 1;
 715                 per_device_sampling_rate = 0;
 716         } else {
 717                 per_sensor_sampling_rate = 0;
 718
 719                 sprintf(sysfs_path, DEVICE_SAMPLING_PATH, dev_num);
 720
 721                 if (sysfs_read_int(sysfs_path, &cur_sampling_rate) != -1)
 722                         per_device_sampling_rate = 1;
 723                 else
 724                         per_device_sampling_rate = 0;
 725         }
 726
 727         if (!per_sensor_sampling_rate && !per_device_sampling_rate) {
 728                 ALOGE("No way to adjust sampling rate on sensor %d\n", s);
 729                 return -ENOSYS;
 730         }
 731
 732         /* Coordinate with others active sensors on the same device, if any */
 733         if (per_device_sampling_rate)
 734                 for (n=0; n<sensor_count; n++)
 735                         if (n != s && sensor_info[n].dev_num == dev_num &&
 736                             sensor_info[n].num_channels &&
 737                             sensor_info[n].enable_count &&
 738                             sensor_info[n].sampling_rate > new_sampling_rate)
 739                                 new_sampling_rate= sensor_info[n].sampling_rate;
 740
 741         /* Check if we have contraints on allowed sampling rates */
 742
 743         sprintf(avail_sysfs_path, DEVICE_AVAIL_FREQ_PATH, dev_num);
 744
 745         if (sysfs_read_str(avail_sysfs_path, freqs_buf, sizeof(freqs_buf)) > 0){
 746                 cursor = freqs_buf;
 747
 748                 /* Decode allowed sampling rates string, ex: "10 20 50 100" */
 749
 750                 /* While we're not at the end of the string */
 751                 while (*cursor && cursor[0]) {
 752
 753                         /* Decode a single integer value */
 754                         n = atoi(cursor);
 755
 756                         if (n > max_supported_rate)
 757                                 max_supported_rate = n;
 758
 759                         /* If this matches the selected rate, we're happy */
 760                         if (new_sampling_rate == n)
 761                                 break;
 762
 763                         /*
 764                          * If we reached a higher value than the desired rate,
 765                          * adjust selected rate so it matches the first higher
 766                          * available one and stop parsing - this makes the
 767                          * assumption that rates are sorted by increasing value
 768                          * in the allowed frequencies string.
 769                          */
 770                         if (n > new_sampling_rate) {
 771                                 ALOGI(
 772                         "Increasing sampling rate on sensor %d from %d to %d\n",
 773                                 s, req_sampling_rate, n);
 774
 775                                 new_sampling_rate = n;
 776                                 break;
 777                         }
 778
 779                         /* Skip digits */
 780                         while (cursor[0] && !isspace(cursor[0]))
 781                                 cursor++;
 782
 783                         /* Skip spaces */
 784                         while (cursor[0] && isspace(cursor[0]))
 785                                         cursor++;
 786                 }
 787         }
 788
 789         /* Cap sampling rate */
 790
 791         limit = 1000000/POLL_MIN_INTERVAL;
 792
 793         if (max_supported_rate && new_sampling_rate > max_supported_rate)
 794                 limit = max_supported_rate;
 795
 796         if (new_sampling_rate > limit) {
 797
 798                 new_sampling_rate = limit;
 799
 800                 ALOGI(  "Can't support %d sampling rate, lowering to %d\n",
 801                         req_sampling_rate, new_sampling_rate);
 802         }
 803
 804         /* If the desired rate is already active we're all set */
 805         if (new_sampling_rate == cur_sampling_rate)
 806                 return 0;
 807
 808         ALOGI("Sensor %d sampling rate switched to %d\n", s, new_sampling_rate);
 809
 810         if (trig_sensors_per_dev[dev_num])
 811                 enable_buffer(dev_num, 0);
 812
 813         sysfs_write_int(sysfs_path, new_sampling_rate);
 814
 815         if (trig_sensors_per_dev[dev_num])
 816                 enable_buffer(dev_num, 1);
 817
 818 exit:
 819         /* Release the polling loop so an updated timeout value gets used */
 820         write(poll_socket_pair[1], "", 1);
 821
 822         return 0;
 823 }
 824
 825
 826
 827 int allocate_control_data (void)
 828 {
 829         int i;
 830         struct epoll_event ev = {0};
 831
 832         for (i=0; i<MAX_DEVICES; i++)
 833                 device_fd[i] = -1;
 834
 835         poll_fd = epoll_create(MAX_DEVICES);
 836
 837         if (poll_fd == -1) {
 838                 ALOGE("Can't create epoll instance for iio sensors!\n");
 839                 return -1;
 840         }
 841
 842         /* Create and add "unblocking" fd to the set of watched fds */
 843
 844         if (socketpair(AF_UNIX, SOCK_STREAM, 0, poll_socket_pair) == -1) {
 845                 ALOGE("Can't create socket pair for iio sensors!\n");
 846                 close(poll_fd);
 847                 return -1;
 848         }
 849
 850         ev.events = EPOLLIN;
 851         ev.data.u32 = INVALID_DEV_NUM;
 852
 853         epoll_ctl(poll_fd, EPOLL_CTL_ADD, poll_socket_pair[0], &ev);
 854
 855         return poll_fd;
 856 }
 857
 858
 859 void delete_control_data (void)
 860 {
 861 }