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, clear up pending data */
 237
 238                 sensor_info[s].report_pending = 0;
 239                 memset(sensor_info[s].report_buffer, 0, MAX_SENSOR_REPORT_SIZE);
 240         }
 241
 242         /* We changed the state of a sensor - adjust per iio device counters */
 243
 244         /* If this is a regular event-driven sensor */
 245         if (sensor_info[s].num_channels) {
 246
 247                         if (enabled)
 248                                 trig_sensors_per_dev[dev_num]++;
 249                         else
 250                                 trig_sensors_per_dev[dev_num]--;
 251
 252                         return 1;
 253                 }
 254
 255         if (enabled) {
 256                 active_poll_sensors++;
 257                 poll_sensors_per_dev[dev_num]++;
 258                 return 1;
 259         }
 260
 261         active_poll_sensors--;
 262         poll_sensors_per_dev[dev_num]--;
 263         return 1;
 264 }
 265
 266
 267 int sensor_activate(int s, int enabled)
 268 {
 269         char sysfs_path[PATH_MAX];
 270         char device_name[PATH_MAX];
 271         char trigger_name[MAX_NAME_SIZE + 16];
 272         int c;
 273         struct epoll_event ev = {0};
 274         int dev_fd;
 275         int ret;
 276         int dev_num = sensor_info[s].dev_num;
 277         int i = sensor_info[s].catalog_index;
 278         int is_poll_sensor = !sensor_info[s].num_channels;
 279
 280         ret = adjust_counters(s, enabled);
 281
 282         /* If the operation was neutral in terms of state, we're done */
 283         if (ret <= 0)
 284                 return ret;
 285
 286         if (!is_poll_sensor) {
 287                 /* Changes have to be made while the buffer is turned off */
 288                 enable_buffer(dev_num, 0);
 289
 290                 /* Configure trigger */
 291                 switch (trig_sensors_per_dev[dev_num]) {
 292                         case 0:
 293                                 setup_trigger(dev_num, "none");
 294                                 break;
 295
 296                         case 1:
 297                                 sprintf(trigger_name, "%s-dev%d",
 298                                         sensor_info[s].internal_name, dev_num);
 299
 300                                 setup_trigger(dev_num, trigger_name);
 301                                 break;
 302
 303                         default:
 304                                 /* The trigger is already set */
 305                                 break;
 306                 }
 307
 308                 /*
 309                  * Turn channels associated to this sensor on or off, and update
 310                  * the channels maps for all sensors associated to this device.
 311                  */
 312                 for (c=0;c<sensor_info[s].num_channels; c++) {
 313                         sprintf(sysfs_path, CHANNEL_PATH "%s",
 314                                 sensor_info[s].dev_num,
 315                                 sensor_catalog[i].channel[c].en_path);
 316
 317                         sysfs_write_int(sysfs_path, enabled);
 318                 }
 319
 320                 /* If there's at least one sensor left */
 321                 if (trig_sensors_per_dev[dev_num]) {
 322                         refresh_sensor_report_maps(dev_num);
 323                         enable_buffer(dev_num, 1);
 324                 }
 325         }
 326
 327         /*
 328          * Make sure we have a fd on the character device ; conversely, close
 329          * the fd if no one is using associated sensor anymore. The assumption
 330          * here is that the underlying driver will power on the relevant
 331          * hardware block while someone hold a fd on the device.
 332          */
 333         dev_fd = device_fd[dev_num];
 334
 335         if (!enabled) {
 336                 if (dev_fd != -1 && !poll_sensors_per_dev[dev_num] &&
 337                         !trig_sensors_per_dev[dev_num]) {
 338                                 /*
 339                                  * Stop watching this fd. This should be a no-op
 340                                  * in case this fd was not in the poll set.
 341                                  */
 342                                 epoll_ctl(poll_fd, EPOLL_CTL_DEL, dev_fd, NULL);
 343
 344                                 close(dev_fd);
 345                                 device_fd[dev_num] = -1;
 346                         }
 347                 return 0;
 348         }
 349
 350         if (dev_fd == -1) {
 351                 /* First enabled sensor on this iio device */
 352                 sprintf(device_name, DEV_FILE_PATH, dev_num);
 353                 dev_fd = open(device_name, O_RDONLY | O_NONBLOCK);
 354
 355                 device_fd[dev_num] = dev_fd;
 356
 357                 if (dev_fd == -1) {
 358                         ALOGE("Could not open fd on %s (%s)\n",
 359                               device_name, strerror(errno));
 360                         adjust_counters(s, 0);
 361                         return -1;
 362                 }
 363
 364                 ALOGV("Opened %s: fd=%d\n", device_name, dev_fd);
 365
 366                 if (!is_poll_sensor) {
 367
 368                         /* Add this iio device fd to the set of watched fds */
 369                         ev.events = EPOLLIN;
 370                         ev.data.u32 = dev_num;
 371
 372                         ret = epoll_ctl(poll_fd, EPOLL_CTL_ADD, dev_fd, &ev);
 373
 374                         if (ret == -1) {
 375                                 ALOGE(  "Failed adding %d to poll set (%s)\n",
 376                                         dev_fd, strerror(errno));
 377                                 return -1;
 378                         }
 379
 380                         /* Note: poll-mode fds are not readable */
 381                 }
 382         }
 383
 384         /* Release the polling loop so an updated timeout gets used */
 385         write(poll_socket_pair[1], "", 1);
 386
 387         return 0;
 388 }
 389
 390
 391 static int integrate_device_report(int dev_num)
 392 {
 393         int len;
 394         int s,c;
 395         unsigned char buf[MAX_SENSOR_REPORT_SIZE * MAX_SENSORS] = { 0 };
 396         int sr_offset;
 397         unsigned char *target;
 398         unsigned char *source;
 399         int size;
 400         int expected_size = 0;
 401
 402         /* There's an incoming report on the specified fd */
 403
 404         if (dev_num < 0 || dev_num >= MAX_DEVICES ||
 405                 !trig_sensors_per_dev[dev_num]) {
 406                 ALOGE("Event reported on unexpected iio device %d\n", dev_num);
 407                 return -1;
 408         }
 409
 410         for (s=0; s<MAX_SENSORS; s++)
 411                 if (sensor_info[s].dev_num == dev_num)
 412                         for (c=0; c<sensor_info[s].num_channels; c++)
 413                                 expected_size += sensor_info[s].channel[c].size;
 414
 415         len = read(device_fd[dev_num], buf, expected_size);
 416
 417         if (len == -1) {
 418                 ALOGE("Could not read report from iio device %d (%s)\n",
 419                       dev_num, strerror(errno));
 420                 return -1;
 421         }
 422
 423         ALOGV("Read %d bytes from iio device %d\n", len, dev_num);
 424
 425         for (s=0; s<MAX_SENSORS; s++)
 426                 if (sensor_info[s].dev_num == dev_num) {
 427                         sr_offset = 0;
 428
 429                         /* Copy data from device to sensor report buffer */
 430                         for (c=0; c<sensor_info[s].num_channels; c++) {
 431
 432                                 target = sensor_info[s].report_buffer +
 433                                         sr_offset;
 434
 435                                 source = buf + sensor_info[s].channel[c].offset;
 436
 437                                 size = sensor_info[s].channel[c].size;
 438
 439                                 memcpy(target, source, size);
 440
 441                                 sr_offset += size;
 442                         }
 443
 444                         if (sensor_info[s].enable_count) {
 445                                 ALOGV("Sensor %d report available (%d bytes)\n",
 446                                       s, sr_offset);
 447
 448                                 sensor_info[s].report_pending = 1;
 449                         }
 450                 }
 451
 452         return 0;
 453 }
 454
 455
 456 static void propagate_sensor_report(int s, struct sensors_event_t* data)
 457 {
 458         /* There's a sensor report pending for this sensor ; transmit it */
 459
 460         int catalog_index = sensor_info[s].catalog_index;
 461         int sensor_type = sensor_catalog[catalog_index].type;
 462         int num_fields;
 463         int c;
 464         unsigned char* current_sample;
 465
 466         memset(data, 0, sizeof(sensors_event_t));
 467
 468         data->version = sizeof(sensors_event_t);
 469         data->sensor = s;
 470         data->type = sensor_type;
 471         data->timestamp = get_timestamp();
 472
 473         switch (sensor_type) {
 474                 case SENSOR_TYPE_ACCELEROMETER:         /* m/s^2        */
 475                 case SENSOR_TYPE_MAGNETIC_FIELD:        /* micro-tesla  */
 476                 case SENSOR_TYPE_ORIENTATION:           /* degrees      */
 477                 case SENSOR_TYPE_GYROSCOPE:             /* radians/s    */
 478                         num_fields = 3;
 479                         break;
 480
 481                 case SENSOR_TYPE_LIGHT:                 /* SI lux units */
 482                 case SENSOR_TYPE_AMBIENT_TEMPERATURE:   /* °C          */
 483                 case SENSOR_TYPE_TEMPERATURE:           /* °C          */
 484                 case SENSOR_TYPE_PROXIMITY:             /* centimeters  */
 485                 case SENSOR_TYPE_PRESSURE:              /* hecto-pascal */
 486                 case SENSOR_TYPE_RELATIVE_HUMIDITY:     /* percent */
 487                         num_fields = 1;
 488                         break;
 489
 490                 case SENSOR_TYPE_ROTATION_VECTOR:
 491                         num_fields = 4;
 492                         break;
 493
 494                 case SENSOR_TYPE_DEVICE_PRIVATE_BASE:   /* hidden for now */
 495                         num_fields = 0;
 496                         break;
 497
 498                 default:
 499                         ALOGE("Unknown sensor type!\n");
 500                         num_fields = 0;
 501                         break;
 502         }
 503
 504         ALOGV("Sample on sensor %d (type %d):\n", s, sensor_type);
 505
 506         /* Take note of current time counter value for rate control purposes */
 507         sensor_info[s].last_integration_ts = get_timestamp();
 508
 509         /* If we're dealing with a poll-mode sensor */
 510         if (!sensor_info[s].num_channels) {
 511
 512                 /* Read values through sysfs rather than from a report buffer */
 513                 for (c=0; c<num_fields; c++) {
 514
 515                         data->data[c] = acquire_immediate_value(s, c);
 516
 517                         ALOGV("\tfield %d: %f\n", c, data->data[c]);
 518                 }
 519
 520                 sensor_info[s].ops.finalize(s, data);
 521                 return;
 522         }
 523
 524         /* Convert the data into the expected Android-level format */
 525
 526         current_sample = sensor_info[s].report_buffer;
 527
 528         for (c=0; c<num_fields; c++) {
 529
 530                 data->data[c] = sensor_info[s].ops.transform
 531                                                         (s, c, current_sample);
 532
 533                 ALOGV("\tfield %d: %f\n", c, data->data[c]);
 534                 current_sample += sensor_info[s].channel[c].size;
 535         }
 536
 537         sensor_info[s].ops.finalize(s, data);
 538 }
 539
 540
 541 static int get_poll_time (void)
 542 {
 543         int64_t target_ts;
 544         int64_t lowest_target_ts;
 545         int64_t current_ts;
 546         int s;
 547
 548         if (!active_poll_sensors)
 549                 return -1;      /* Infinite wait */
 550
 551         /* Check if we should schedule a poll-mode sensor event delivery */
 552
 553         lowest_target_ts = INT64_MAX;
 554
 555         for (s=0; s<sensor_count; s++)
 556                 if (sensor_info[s].enable_count &&
 557                     sensor_info[s].sampling_rate &&
 558                     !sensor_info[s].num_channels) {
 559                                 target_ts = sensor_info[s].last_integration_ts +
 560                                       1000000000LL/sensor_info[s].sampling_rate;
 561
 562                                 if (target_ts < lowest_target_ts)
 563                                         lowest_target_ts = target_ts;
 564                         }
 565
 566         if (lowest_target_ts == INT64_MAX)
 567                 return -1;
 568
 569         current_ts = get_timestamp();
 570
 571         if (lowest_target_ts <= current_ts)
 572                 return 0;
 573
 574         return (lowest_target_ts - current_ts)/1000000; /* ms */
 575 }
 576
 577
 578 static void acknowledge_release (void)
 579 {
 580         /* A write to our socket circuit was performed to release epoll */
 581         char buf;
 582         read(poll_socket_pair[0], &buf, 1);
 583 }
 584
 585
 586 int sensor_poll(struct sensors_event_t* data, int count)
 587 {
 588         int s;
 589         int i;
 590         int nfds;
 591         int delta;
 592         struct epoll_event ev[MAX_DEVICES];
 593
 594         /* Get one or more events from our collection of sensors */
 595
 596 return_first_available_sensor_report:
 597
 598         /* If there's at least one available report */
 599         for (s=0; s<sensor_count; s++)
 600                 if (sensor_info[s].report_pending) {
 601
 602                         /* Return that up */
 603                         propagate_sensor_report(s, data);
 604                         sensor_info[s].report_pending = 0;
 605                         ALOGV("Report on sensor %d\n", s);
 606                         return 1;
 607                 }
 608 await_event:
 609
 610         /* Keep a minimum time interval between poll operations */
 611         delta = (get_timestamp() - last_poll_exit_ts)/1000;
 612
 613         if (delta > 0 && delta < POLL_MIN_INTERVAL)
 614                 usleep(POLL_MIN_INTERVAL - delta);
 615
 616         ALOGV("Awaiting sensor data\n");
 617
 618         nfds = epoll_wait(poll_fd, ev, MAX_DEVICES, get_poll_time());
 619
 620         last_poll_exit_ts = get_timestamp();
 621
 622         if (nfds == -1) {
 623                 ALOGI("epoll_wait returned -1 (%s)\n", strerror(errno));
 624                 goto await_event;
 625         }
 626
 627         ALOGV("%d fds signalled\n", nfds);
 628
 629         /* For each of the devices for which a report is available */
 630         for (i=0; i<nfds; i++)
 631                 if (ev[i].events == EPOLLIN) {
 632                         if (ev[i].data.u32 == INVALID_DEV_NUM) {
 633                                 acknowledge_release();
 634                                 goto await_event;
 635                         } else
 636                                 /* Read report */
 637                                 integrate_device_report(ev[i].data.u32);
 638                 }
 639
 640         /* It's a good time to invalidate poll-mode sensor values */
 641         if (active_poll_sensors)
 642                 for (s=0; s<sensor_count; s++)
 643                         if (sensor_info[s].enable_count &&
 644                                 !sensor_info[s].num_channels)
 645                                         sensor_info[s].report_pending = 1;
 646
 647         goto return_first_available_sensor_report;
 648 }
 649
 650
 651 int sensor_set_delay(int s, int64_t ns)
 652 {
 653         /* Set the rate at which a specific sensor should report events */
 654
 655         /* See Android sensors.h for indication on sensor trigger modes */
 656
 657         char sysfs_path[PATH_MAX];
 658         char avail_sysfs_path[PATH_MAX];
 659         int dev_num             =       sensor_info[s].dev_num;
 660         int i                   =       sensor_info[s].catalog_index;
 661         const char *prefix      =       sensor_catalog[i].tag;
 662         int new_sampling_rate;  /* Granted sampling rate after arbitration   */
 663         int cur_sampling_rate;  /* Currently used sampling rate              */
 664         int req_sampling_rate;  /* Requested ; may be different from granted */
 665         int per_sensor_sampling_rate;
 666         int per_device_sampling_rate;
 667         int max_supported_rate = 0;
 668         int limit;
 669         char freqs_buf[100];
 670         char* cursor;
 671         int n;
 672
 673         if (!ns) {
 674                 ALOGE("Rejecting zero delay request on sensor %d\n", s);
 675                 return -EINVAL;
 676         }
 677
 678         new_sampling_rate = req_sampling_rate = (int) (1000000000L/ns);
 679
 680         if (!new_sampling_rate) {
 681                 ALOGI("Sub-HZ sampling rate requested on on sensor %d\n", s);
 682                 new_sampling_rate = 1;
 683         }
 684
 685         sensor_info[s].sampling_rate = new_sampling_rate;
 686
 687         /* If we're dealing with a poll-mode sensor, release poll and return */
 688         if (!sensor_info[s].num_channels)
 689                 goto exit;
 690
 691         sprintf(sysfs_path, SENSOR_SAMPLING_PATH, dev_num, prefix);
 692
 693         if (sysfs_read_int(sysfs_path, &cur_sampling_rate) != -1) {
 694                 per_sensor_sampling_rate = 1;
 695                 per_device_sampling_rate = 0;
 696         } else {
 697                 per_sensor_sampling_rate = 0;
 698
 699                 sprintf(sysfs_path, DEVICE_SAMPLING_PATH, dev_num);
 700
 701                 if (sysfs_read_int(sysfs_path, &cur_sampling_rate) != -1)
 702                         per_device_sampling_rate = 1;
 703                 else
 704                         per_device_sampling_rate = 0;
 705         }
 706
 707         if (!per_sensor_sampling_rate && !per_device_sampling_rate) {
 708                 ALOGE("No way to adjust sampling rate on sensor %d\n", s);
 709                 return -ENOSYS;
 710         }
 711
 712         /* Coordinate with others active sensors on the same device, if any */
 713         if (per_device_sampling_rate)
 714                 for (n=0; n<sensor_count; n++)
 715                         if (n != s && sensor_info[n].dev_num == dev_num &&
 716                             sensor_info[n].num_channels &&
 717                             sensor_info[n].enable_count &&
 718                             sensor_info[n].sampling_rate > new_sampling_rate)
 719                                 new_sampling_rate= sensor_info[n].sampling_rate;
 720
 721         /* Check if we have contraints on allowed sampling rates */
 722
 723         sprintf(avail_sysfs_path, DEVICE_AVAIL_FREQ_PATH, dev_num);
 724
 725         if (sysfs_read_str(avail_sysfs_path, freqs_buf, sizeof(freqs_buf)) > 0){
 726                 cursor = freqs_buf;
 727
 728                 /* Decode allowed sampling rates string, ex: "10 20 50 100" */
 729
 730                 /* While we're not at the end of the string */
 731                 while (*cursor && cursor[0]) {
 732
 733                         /* Decode a single integer value */
 734                         n = atoi(cursor);
 735
 736                         if (n > max_supported_rate)
 737                                 max_supported_rate = n;
 738
 739                         /* If this matches the selected rate, we're happy */
 740                         if (new_sampling_rate == n)
 741                                 break;
 742
 743                         /*
 744                          * If we reached a higher value than the desired rate,
 745                          * adjust selected rate so it matches the first higher
 746                          * available one and stop parsing - this makes the
 747                          * assumption that rates are sorted by increasing value
 748                          * in the allowed frequencies string.
 749                          */
 750                         if (n > new_sampling_rate) {
 751                                 ALOGI(
 752                         "Increasing sampling rate on sensor %d from %d to %d\n",
 753                                 s, req_sampling_rate, n);
 754
 755                                 new_sampling_rate = n;
 756                                 break;
 757                         }
 758
 759                         /* Skip digits */
 760                         while (cursor[0] && !isspace(cursor[0]))
 761                                 cursor++;
 762
 763                         /* Skip spaces */
 764                         while (cursor[0] && isspace(cursor[0]))
 765                                         cursor++;
 766                 }
 767         }
 768
 769         /* Cap sampling rate */
 770
 771         limit = 1000000/POLL_MIN_INTERVAL;
 772
 773         if (max_supported_rate && new_sampling_rate > max_supported_rate)
 774                 limit = max_supported_rate;
 775
 776         if (new_sampling_rate > limit) {
 777
 778                 new_sampling_rate = limit;
 779
 780                 ALOGI(  "Can't support %d sampling rate, lowering to %d\n",
 781                         req_sampling_rate, new_sampling_rate);
 782         }
 783
 784         /* If the desired rate is already active we're all set */
 785         if (new_sampling_rate == cur_sampling_rate)
 786                 return 0;
 787
 788         ALOGI("Sensor %d sampling rate switched to %d\n", s, new_sampling_rate);
 789
 790         if (trig_sensors_per_dev[dev_num])
 791                 enable_buffer(dev_num, 0);
 792
 793         sysfs_write_int(sysfs_path, new_sampling_rate);
 794
 795         if (trig_sensors_per_dev[dev_num])
 796                 enable_buffer(dev_num, 1);
 797
 798 exit:
 799         /* Release the polling loop so an updated timeout value gets used */
 800         write(poll_socket_pair[1], "", 1);
 801
 802         return 0;
 803 }
 804
 805
 806
 807 int allocate_control_data (void)
 808 {
 809         int i;
 810         struct epoll_event ev = {0};
 811
 812         for (i=0; i<MAX_DEVICES; i++)
 813                 device_fd[i] = -1;
 814
 815         poll_fd = epoll_create(MAX_DEVICES);
 816
 817         if (poll_fd == -1) {
 818                 ALOGE("Can't create epoll instance for iio sensors!\n");
 819                 return -1;
 820         }
 821
 822         /* Create and add "unblocking" fd to the set of watched fds */
 823
 824         if (socketpair(AF_UNIX, SOCK_STREAM, 0, poll_socket_pair) == -1) {
 825                 ALOGE("Can't create socket pair for iio sensors!\n");
 826                 close(poll_fd);
 827                 return -1;
 828         }
 829
 830         ev.events = EPOLLIN;
 831         ev.data.u32 = INVALID_DEV_NUM;
 832
 833         epoll_ctl(poll_fd, EPOLL_CTL_ADD, poll_socket_pair[0], &ev);
 834
 835         return poll_fd;
 836 }
 837
 838
 839 void delete_control_data (void)
 840 {
 841 }