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