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