control.c

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