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 <pthread.h>
   9 #include <sys/epoll.h>
  10 #include <sys/socket.h>
  11 #include <utils/Log.h>
  12 #include <hardware/sensors.h>
  13 #include "control.h"
  14 #include "enumeration.h"
  15 #include "utils.h"
  16 #include "transform.h"
  17 #include "calibration.h"
  18 #include "description.h"
  19
  20 /* Currently active sensors count, per device */
  21 static int poll_sensors_per_dev[MAX_DEVICES];   /* poll-mode sensors */
  22 static int trig_sensors_per_dev[MAX_DEVICES];   /* trigger, event based */
  23
  24 static int device_fd[MAX_DEVICES];   /* fd on the /dev/iio:deviceX file */
  25
  26 static int poll_fd; /* epoll instance covering all enabled sensors */
  27
  28 static int active_poll_sensors; /* Number of enabled poll-mode sensors */
  29
  30 /* We use pthread condition variables to get worker threads out of sleep */
  31 static pthread_cond_t  thread_release_cond      [MAX_SENSORS];
  32 static pthread_mutex_t thread_release_mutex     [MAX_SENSORS];
  33
  34 /*
  35  * We associate tags to each of our poll set entries. These tags have the
  36  * following values:
  37  * - a iio device number if the fd is a iio character device fd
  38  * - THREAD_REPORT_TAG_BASE + sensor handle if the fd is the receiving end of a
  39  *   pipe used by a sysfs data acquisition thread
  40  *  */
  41 #define THREAD_REPORT_TAG_BASE  0x00010000
  42
  43
  44 static int enable_buffer(int dev_num, int enabled)
  45 {
  46         char sysfs_path[PATH_MAX];
  47
  48         sprintf(sysfs_path, ENABLE_PATH, dev_num);
  49
  50         /* Low level, non-multiplexed, enable/disable routine */
  51         return sysfs_write_int(sysfs_path, enabled);
  52 }
  53
  54
  55 static int setup_trigger(int dev_num, const char* trigger_val)
  56 {
  57         char sysfs_path[PATH_MAX];
  58
  59         sprintf(sysfs_path, TRIGGER_PATH, dev_num);
  60
  61         return sysfs_write_str(sysfs_path, trigger_val);
  62 }
  63
  64
  65 void build_sensor_report_maps(int dev_num)
  66 {
  67         /*
  68          * Read sysfs files from a iio device's scan_element directory, and
  69          * build a couple of tables from that data. These tables will tell, for
  70          * each sensor, where to gather relevant data in a device report, i.e.
  71          * the structure that we read from the /dev/iio:deviceX file in order to
  72          * sensor report, itself being the data that we return to Android when a
  73          * sensor poll completes. The mapping should be straightforward in the
  74          * case where we have a single sensor active per iio device but, this is
  75          * not the general case. In general several sensors can be handled
  76          * through a single iio device, and the _en, _index and _type syfs
  77          * entries all concur to paint a picture of what the structure of the
  78          * device report is.
  79          */
  80
  81         int s;
  82         int c;
  83         int n;
  84         int i;
  85         int ch_index;
  86         char* ch_spec;
  87         char spec_buf[MAX_TYPE_SPEC_LEN];
  88         struct datum_info_t* ch_info;
  89         int size;
  90         char sysfs_path[PATH_MAX];
  91         int known_channels;
  92         int offset;
  93         int channel_size_from_index[MAX_SENSORS * MAX_CHANNELS] = { 0 };
  94         int sensor_handle_from_index[MAX_SENSORS * MAX_CHANNELS] = { 0 };
  95         int channel_number_from_index[MAX_SENSORS * MAX_CHANNELS] = { 0 };
  96
  97         known_channels = 0;
  98
  99         /* For each sensor that is linked to this device */
 100         for (s=0; s<sensor_count; s++) {
 101                 if (sensor_info[s].dev_num != dev_num)
 102                         continue;
 103
 104                 i = sensor_info[s].catalog_index;
 105
 106                 /* Read channel details through sysfs attributes */
 107                 for (c=0; c<sensor_info[s].num_channels; c++) {
 108
 109                         /* Read _type file */
 110                         sprintf(sysfs_path, CHANNEL_PATH "%s",
 111                                 sensor_info[s].dev_num,
 112                                 sensor_catalog[i].channel[c].type_path);
 113
 114                         n = sysfs_read_str(sysfs_path, spec_buf,
 115                                                 sizeof(spec_buf));
 116
 117                         if (n == -1) {
 118                                         ALOGW(  "Failed to read type: %s\n",
 119                                         sysfs_path);
 120                                         continue;
 121                                 }
 122
 123                         ch_spec = sensor_info[s].channel[c].type_spec;
 124
 125                         memcpy(ch_spec, spec_buf, sizeof(spec_buf));
 126
 127                         ch_info = &sensor_info[s].channel[c].type_info;
 128
 129                         size = decode_type_spec(ch_spec, ch_info);
 130
 131                         /* Read _index file */
 132                         sprintf(sysfs_path, CHANNEL_PATH "%s",
 133                                 sensor_info[s].dev_num,
 134                                 sensor_catalog[i].channel[c].index_path);
 135
 136                         n = sysfs_read_int(sysfs_path, &ch_index);
 137
 138                         if (n == -1) {
 139                                         ALOGW(  "Failed to read index: %s\n",
 140                                                 sysfs_path);
 141                                         continue;
 142                                 }
 143
 144                         if (ch_index >= MAX_SENSORS) {
 145                                 ALOGE("Index out of bounds!: %s\n", sysfs_path);
 146                                 continue;
 147                         }
 148
 149                         /* Record what this index is about */
 150
 151                         sensor_handle_from_index [ch_index] = s;
 152                         channel_number_from_index[ch_index] = c;
 153                         channel_size_from_index  [ch_index] = size;
 154
 155                         known_channels++;
 156                 }
 157
 158                 /* Stop sampling - if we are recovering from hal restart */
 159                 enable_buffer(dev_num, 0);
 160                 setup_trigger(dev_num, "\n");
 161
 162                 /* Turn on channels we're aware of */
 163                 for (c=0;c<sensor_info[s].num_channels; c++) {
 164                         sprintf(sysfs_path, CHANNEL_PATH "%s",
 165                                 sensor_info[s].dev_num,
 166                                 sensor_catalog[i].channel[c].en_path);
 167                         sysfs_write_int(sysfs_path, 1);
 168                 }
 169         }
 170
 171         ALOGI("Found %d channels on iio device %d\n", known_channels, dev_num);
 172
 173         /*
 174          * Now that we know which channels are defined, 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         int catalog_index = sensor_info[s].catalog_index;
 214         int sensor_type = sensor_catalog[catalog_index].type;
 215
 216         /* Refcount per sensor, in terms of enable count */
 217         if (enabled) {
 218                 ALOGI("Enabling sensor %d (iio device %d: %s)\n",
 219                         s, dev_num, sensor_info[s].friendly_name);
 220
 221                 sensor_info[s].enable_count++;
 222
 223                 if (sensor_info[s].enable_count > 1)
 224                         return 0; /* The sensor was, and remains, in use */
 225
 226                 switch (sensor_type) {
 227                         case SENSOR_TYPE_MAGNETIC_FIELD:
 228                                 compass_read_data(&sensor_info[s]);
 229                                 break;
 230
 231                         case SENSOR_TYPE_GYROSCOPE:
 232                         case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
 233                                 gyro_cal_init(&sensor_info[s]);
 234                                 break;
 235                 }
 236         } else {
 237                 if (sensor_info[s].enable_count == 0)
 238                         return -1; /* Spurious disable call */
 239
 240                 ALOGI("Disabling sensor %d (iio device %d: %s)\n", s, dev_num,
 241                       sensor_info[s].friendly_name);
 242
 243                 sensor_info[s].enable_count--;
 244
 245                 if (sensor_info[s].enable_count > 0)
 246                         return 0; /* The sensor was, and remains, in use */
 247
 248                 /* Sensor disabled, lower report available flag */
 249                 sensor_info[s].report_pending = 0;
 250
 251                 if (sensor_type == SENSOR_TYPE_MAGNETIC_FIELD)
 252                         compass_store_data(&sensor_info[s]);
 253         }
 254
 255
 256         /* If uncalibrated type and pair is already active don't adjust counters */
 257         if (sensor_type == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED &&
 258                 sensor_info[sensor_info[s].pair_idx].enable_count != 0)
 259                         return 0;
 260
 261         /* We changed the state of a sensor - adjust per iio device counters */
 262
 263         /* If this is a regular event-driven sensor */
 264         if (sensor_info[s].num_channels) {
 265
 266                         if (enabled)
 267                                 trig_sensors_per_dev[dev_num]++;
 268                         else
 269                                 trig_sensors_per_dev[dev_num]--;
 270
 271                         return 1;
 272                 }
 273
 274         if (enabled) {
 275                 active_poll_sensors++;
 276                 poll_sensors_per_dev[dev_num]++;
 277                 return 1;
 278         }
 279
 280         active_poll_sensors--;
 281         poll_sensors_per_dev[dev_num]--;
 282         return 1;
 283 }
 284
 285
 286 static int get_field_count (int s)
 287 {
 288         int catalog_index = sensor_info[s].catalog_index;
 289         int sensor_type   = sensor_catalog[catalog_index].type;
 290
 291         switch (sensor_type) {
 292                 case SENSOR_TYPE_ACCELEROMETER:         /* m/s^2        */
 293                 case SENSOR_TYPE_MAGNETIC_FIELD:        /* micro-tesla  */
 294                 case SENSOR_TYPE_ORIENTATION:           /* degrees      */
 295                 case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
 296                 case SENSOR_TYPE_GYROSCOPE:             /* radians/s    */
 297                         return 3;
 298
 299                 case SENSOR_TYPE_LIGHT:                 /* SI lux units */
 300                 case SENSOR_TYPE_AMBIENT_TEMPERATURE:   /* °C          */
 301                 case SENSOR_TYPE_TEMPERATURE:           /* °C          */
 302                 case SENSOR_TYPE_PROXIMITY:             /* centimeters  */
 303                 case SENSOR_TYPE_PRESSURE:              /* hecto-pascal */
 304                 case SENSOR_TYPE_RELATIVE_HUMIDITY:     /* percent */
 305                         return 1;
 306
 307                 case SENSOR_TYPE_ROTATION_VECTOR:
 308                         return  4;
 309
 310                 default:
 311                         ALOGE("Unknown sensor type!\n");
 312                         return 0;                       /* Drop sample */
 313         }
 314 }
 315
 316
 317 static void time_add(struct timespec *out, struct timespec *in, int64_t ns)
 318 {
 319         int64_t target_ts = 1000000000LL * in->tv_sec + in->tv_nsec + ns;
 320
 321         out->tv_sec = target_ts / 1000000000;
 322         out->tv_nsec = target_ts % 1000000000;
 323 }
 324
 325
 326 static void* acquisition_routine (void* param)
 327 {
 328         /*
 329          * Data acquisition routine run in a dedicated thread, covering a single
 330          * sensor. This loop will periodically retrieve sampling data through
 331          * sysfs, then package it as a sample and transfer it to our master poll
 332          * loop through a report fd. Checks for a cancellation signal quite
 333          * frequently, as the thread may be disposed of at any time. Note that
 334          * Bionic does not provide pthread_cancel / pthread_testcancel...
 335          */
 336
 337         int s = (int) param;
 338         int report_fd;
 339         int num_fields;
 340         struct sensors_event_t data = {0};
 341         int c;
 342         int sampling_rate;
 343         int ret;
 344         struct timespec entry_time;
 345         struct timespec target_time;
 346         int64_t period;
 347
 348         ALOGV("Entering data acquisition thread for sensor %d\n", s);
 349
 350         if (s < 0 || s >= sensor_count) {
 351                 ALOGE("Invalid sensor handle!\n");
 352                 return NULL;
 353         }
 354
 355         if (!sensor_info[s].sampling_rate) {
 356                 ALOGE("Zero rate in acquisition routine for sensor %d\n", s);
 357                 return NULL;
 358         }
 359
 360         num_fields = get_field_count(s);
 361
 362         /*
 363          * Each condition variable is associated to a mutex that has to be
 364          * locked by the thread that's waiting on it. We use these condition
 365          * variables to get the acquisition threads out of sleep quickly after
 366          * the sampling rate is adjusted, or the sensor is disabled.
 367          */
 368         pthread_mutex_lock(&thread_release_mutex[s]);
 369
 370         while (1) {
 371                 /* Pinpoint the moment we start sampling */
 372                 clock_gettime(CLOCK_REALTIME, &entry_time);
 373
 374                 ALOGV("Acquiring sample data for sensor %d through sysfs\n", s);
 375
 376                 /* Read values through sysfs */
 377                 for (c=0; c<num_fields; c++) {
 378                         data.data[c] = acquire_immediate_value(s, c);
 379
 380                         /* Check and honor termination requests */
 381                         if (sensor_info[s].thread_data_fd[1] == -1)
 382                                 goto exit;
 383
 384                         ALOGV("\tfield %d: %f\n", c, data.data[c]);
 385
 386                 }
 387
 388                 /* If the sample looks good */
 389                 if (sensor_info[s].ops.finalize(s, &data)) {
 390
 391                         /* Pipe it for transmission to poll loop */
 392                         ret = write(    sensor_info[s].thread_data_fd[1],
 393                                         data.data,
 394                                         num_fields * sizeof(float));
 395                 }
 396
 397                 /* Check and honor termination requests */
 398                 if (sensor_info[s].thread_data_fd[1] == -1)
 399                         goto exit;
 400
 401
 402                 period = 1000000000LL / sensor_info[s].sampling_rate;
 403
 404                 time_add(&target_time, &entry_time, period);
 405
 406                 /*
 407                  * Wait until the sampling time elapses, or a rate change is
 408                  * signaled, or a thread exit is requested.
 409                  */
 410                 ret = pthread_cond_timedwait(   &thread_release_cond[s],
 411                                                 &thread_release_mutex[s],
 412                                                 &target_time);
 413
 414                 /* Check and honor termination requests */
 415                 if (sensor_info[s].thread_data_fd[1] == -1)
 416                                 goto exit;
 417         }
 418
 419 exit:
 420         ALOGV("Acquisition thread for S%d exiting\n", s);
 421         pthread_mutex_unlock(&thread_release_mutex[s]);
 422         pthread_exit(0);
 423         return NULL;
 424 }
 425
 426
 427 static void start_acquisition_thread (int s)
 428 {
 429         int incoming_data_fd;
 430         int ret;
 431
 432         struct epoll_event ev = {0};
 433
 434         ALOGV("Initializing acquisition context for sensor %d\n", s);
 435
 436         /* Create condition variable and mutex for quick thread release */
 437         ret = pthread_cond_init(&thread_release_cond[s], NULL);
 438         ret = pthread_mutex_init(&thread_release_mutex[s], NULL);
 439
 440         /* Create a pipe for inter thread communication */
 441         ret = pipe(sensor_info[s].thread_data_fd);
 442
 443         incoming_data_fd = sensor_info[s].thread_data_fd[0];
 444
 445         ev.events = EPOLLIN;
 446         ev.data.u32 = THREAD_REPORT_TAG_BASE + s;
 447
 448         /* Add incoming side of pipe to our poll set, with a suitable tag */
 449         ret = epoll_ctl(poll_fd, EPOLL_CTL_ADD, incoming_data_fd , &ev);
 450
 451         /* Create and start worker thread */
 452         ret = pthread_create(   &sensor_info[s].acquisition_thread,
 453                                 NULL,
 454                                 acquisition_routine,
 455                                 (void*) s);
 456 }
 457
 458
 459 static void stop_acquisition_thread (int s)
 460 {
 461         int incoming_data_fd = sensor_info[s].thread_data_fd[0];
 462         int outgoing_data_fd = sensor_info[s].thread_data_fd[1];
 463
 464         ALOGV("Tearing down acquisition context for sensor %d\n", s);
 465
 466         /* Delete the incoming side of the pipe from our poll set */
 467         epoll_ctl(poll_fd, EPOLL_CTL_DEL, incoming_data_fd, NULL);
 468
 469         /* Mark the pipe ends as invalid ; that's a cheap exit flag */
 470         sensor_info[s].thread_data_fd[0] = -1;
 471         sensor_info[s].thread_data_fd[1] = -1;
 472
 473         /* Close both sides of our pipe */
 474         close(incoming_data_fd);
 475         close(outgoing_data_fd);
 476
 477         /* Stop acquisition thread and clean up thread handle */
 478         pthread_cond_signal(&thread_release_cond[s]);
 479         pthread_join(sensor_info[s].acquisition_thread, NULL);
 480
 481         /* Clean up our sensor descriptor */
 482         sensor_info[s].acquisition_thread = -1;
 483
 484         /* Delete condition variable and mutex */
 485         pthread_cond_destroy(&thread_release_cond[s]);
 486         pthread_mutex_destroy(&thread_release_mutex[s]);
 487 }
 488
 489
 490 int sensor_activate(int s, int enabled)
 491 {
 492         char device_name[PATH_MAX];
 493         char trigger_name[MAX_NAME_SIZE + 16];
 494         int c;
 495         struct epoll_event ev = {0};
 496         int dev_fd;
 497         int ret;
 498         int dev_num = sensor_info[s].dev_num;
 499         int i = sensor_info[s].catalog_index;
 500         int is_poll_sensor = !sensor_info[s].num_channels;
 501
 502         /* If we want to activate gyro calibrated and gyro uncalibrated is activated
 503          * Deactivate gyro uncalibrated - Uncalibrated releases handler
 504          * Activate gyro calibrated     - Calibrated has handler
 505          * Reactivate gyro uncalibrated - Uncalibrated gets data from calibrated */
 506
 507         /* If we want to deactivate gyro calibrated and gyro uncalibrated is active
 508          * Deactivate gyro uncalibrated - Uncalibrated no longer gets data from handler
 509          * Deactivate gyro calibrated   - Calibrated releases handler
 510          * Reactivate gyro uncalibrated - Uncalibrated has handler */
 511
 512         if (sensor_catalog[sensor_info[s].catalog_index].type == SENSOR_TYPE_GYROSCOPE &&
 513                 sensor_info[s].pair_idx && sensor_info[sensor_info[s].pair_idx].enable_count != 0) {
 514
 515                                 sensor_activate(sensor_info[s].pair_idx, 0);
 516                                 ret = sensor_activate(s, enabled);
 517                                 sensor_activate(sensor_info[s].pair_idx, 1);
 518                                 return ret;
 519         }
 520
 521         ret = adjust_counters(s, enabled);
 522
 523         /* If the operation was neutral in terms of state, we're done */
 524         if (ret <= 0)
 525                 return ret;
 526
 527
 528         if (!is_poll_sensor) {
 529
 530                 /* Stop sampling */
 531                 enable_buffer(dev_num, 0);
 532                 setup_trigger(dev_num, "\n");
 533
 534                 /* If there's at least one sensor enabled on this iio device */
 535                 if (trig_sensors_per_dev[dev_num]) {
 536                         sprintf(trigger_name, "%s-dev%d",
 537                                         sensor_info[s].internal_name, dev_num);
 538
 539                         /* Start sampling */
 540                         setup_trigger(dev_num, trigger_name);
 541                         enable_buffer(dev_num, 1);
 542                 }
 543         }
 544
 545         /*
 546          * Make sure we have a fd on the character device ; conversely, close
 547          * the fd if no one is using associated sensors anymore. The assumption
 548          * here is that the underlying driver will power on the relevant
 549          * hardware block while someone holds a fd on the device.
 550          */
 551         dev_fd = device_fd[dev_num];
 552
 553         if (!enabled) {
 554                 if (is_poll_sensor)
 555                         stop_acquisition_thread(s);
 556
 557                 if (dev_fd != -1 && !poll_sensors_per_dev[dev_num] &&
 558                         !trig_sensors_per_dev[dev_num]) {
 559                                 /*
 560                                  * Stop watching this fd. This should be a no-op
 561                                  * in case this fd was not in the poll set.
 562                                  */
 563                                 epoll_ctl(poll_fd, EPOLL_CTL_DEL, dev_fd, NULL);
 564
 565                                 close(dev_fd);
 566                                 device_fd[dev_num] = -1;
 567                         }
 568                 return 0;
 569         }
 570
 571         if (dev_fd == -1) {
 572                 /* First enabled sensor on this iio device */
 573                 sprintf(device_name, DEV_FILE_PATH, dev_num);
 574                 dev_fd = open(device_name, O_RDONLY | O_NONBLOCK);
 575
 576                 device_fd[dev_num] = dev_fd;
 577
 578                 if (dev_fd == -1) {
 579                         ALOGE("Could not open fd on %s (%s)\n",
 580                               device_name, strerror(errno));
 581                         adjust_counters(s, 0);
 582                         return -1;
 583                 }
 584
 585                 ALOGV("Opened %s: fd=%d\n", device_name, dev_fd);
 586
 587                 if (!is_poll_sensor) {
 588
 589                         /* Add this iio device fd to the set of watched fds */
 590                         ev.events = EPOLLIN;
 591                         ev.data.u32 = dev_num;
 592
 593                         ret = epoll_ctl(poll_fd, EPOLL_CTL_ADD, dev_fd, &ev);
 594
 595                         if (ret == -1) {
 596                                 ALOGE(  "Failed adding %d to poll set (%s)\n",
 597                                         dev_fd, strerror(errno));
 598                                 return -1;
 599                         }
 600
 601                         /* Note: poll-mode fds are not readable */
 602                 }
 603         }
 604
 605         /* Ensure that on-change sensors send at least one event after enable */
 606         sensor_info[s].prev_val = -1;
 607
 608         if (is_poll_sensor)
 609                 start_acquisition_thread(s);
 610
 611         return 0;
 612 }
 613
 614
 615 static int integrate_device_report(int dev_num)
 616 {
 617         int len;
 618         int s,c;
 619         unsigned char buf[MAX_SENSOR_REPORT_SIZE] = { 0 };
 620         int sr_offset;
 621         unsigned char *target;
 622         unsigned char *source;
 623         int size;
 624         int64_t ts;
 625
 626         /* There's an incoming report on the specified iio device char dev fd */
 627
 628         if (dev_num < 0 || dev_num >= MAX_DEVICES) {
 629                 ALOGE("Event reported on unexpected iio device %d\n", dev_num);
 630                 return -1;
 631         }
 632
 633         if (device_fd[dev_num] == -1) {
 634                 ALOGE("Ignoring stale report on iio device %d\n", dev_num);
 635                 return -1;
 636         }
 637
 638         ts = get_timestamp();
 639
 640         len = read(device_fd[dev_num], buf, MAX_SENSOR_REPORT_SIZE);
 641
 642         if (len == -1) {
 643                 ALOGE("Could not read report from iio device %d (%s)\n",
 644                       dev_num, strerror(errno));
 645                 return -1;
 646         }
 647
 648         ALOGV("Read %d bytes from iio device %d\n", len, dev_num);
 649
 650         for (s=0; s<MAX_SENSORS; s++)
 651                 if (sensor_info[s].dev_num == dev_num &&
 652                     sensor_info[s].enable_count) {
 653
 654                         sr_offset = 0;
 655
 656                         /* Copy data from device to sensor report buffer */
 657                         for (c=0; c<sensor_info[s].num_channels; c++) {
 658
 659                                 target = sensor_info[s].report_buffer +
 660                                         sr_offset;
 661
 662                                 source = buf + sensor_info[s].channel[c].offset;
 663
 664                                 size = sensor_info[s].channel[c].size;
 665
 666                                 memcpy(target, source, size);
 667
 668                                 sr_offset += size;
 669                         }
 670
 671                         ALOGV("Sensor %d report available (%d bytes)\n", s,
 672                               sr_offset);
 673
 674                         sensor_info[s].report_ts = ts;
 675                         sensor_info[s].report_pending = 1;
 676                 }
 677
 678         return 0;
 679 }
 680
 681
 682 static int propagate_sensor_report(int s, struct sensors_event_t  *data)
 683 {
 684         /* There's a sensor report pending for this sensor ; transmit it */
 685
 686         int catalog_index = sensor_info[s].catalog_index;
 687         int sensor_type   = sensor_catalog[catalog_index].type;
 688         int num_fields    = get_field_count(s);
 689         int c;
 690         unsigned char* current_sample;
 691
 692         /* If there's nothing to return... we're done */
 693         if (!num_fields)
 694                 return 0;
 695
 696
 697         /* Only return uncalibrated event if also gyro active */
 698         if (sensor_type == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED &&
 699                 sensor_info[sensor_info[s].pair_idx].enable_count != 0)
 700                         return 0;
 701
 702         memset(data, 0, sizeof(sensors_event_t));
 703
 704         data->version   = sizeof(sensors_event_t);
 705         data->sensor    = s;
 706         data->type      = sensor_type;
 707         data->timestamp = sensor_info[s].report_ts;
 708
 709         ALOGV("Sample on sensor %d (type %d):\n", s, sensor_type);
 710
 711         current_sample = sensor_info[s].report_buffer;
 712
 713         /* If this is a poll sensor */
 714         if (!sensor_info[s].num_channels) {
 715                 /* Use the data provided by the acquisition thread */
 716                 ALOGV("Reporting data from worker thread for S%d\n", s);
 717                 memcpy(data->data, current_sample, num_fields * sizeof(float));
 718                 return 1;
 719         }
 720
 721         /* Convert the data into the expected Android-level format */
 722         for (c=0; c<num_fields; c++) {
 723
 724                 data->data[c] = sensor_info[s].ops.transform
 725                                                         (s, c, current_sample);
 726
 727                 ALOGV("\tfield %d: %f\n", c, data->data[c]);
 728                 current_sample += sensor_info[s].channel[c].size;
 729         }
 730
 731         /*
 732          * The finalize routine, in addition to its late sample processing duty,
 733          * has the final say on whether or not the sample gets sent to Android.
 734          */
 735         return sensor_info[s].ops.finalize(s, data);
 736 }
 737
 738
 739 static void integrate_thread_report (uint32_t tag)
 740 {
 741         int s = tag - THREAD_REPORT_TAG_BASE;
 742         int len;
 743         int expected_len;
 744
 745         expected_len = get_field_count(s) * sizeof(float);
 746
 747         len = read(sensor_info[s].thread_data_fd[0],
 748                    sensor_info[s].report_buffer,
 749                    expected_len);
 750
 751         if (len == expected_len) {
 752                 sensor_info[s].report_ts = get_timestamp();
 753                 sensor_info[s].report_pending = 1;
 754         }
 755 }
 756
 757
 758 int sensor_poll(struct sensors_event_t* data, int count)
 759 {
 760         int s;
 761         int i;
 762         int nfds;
 763         struct epoll_event ev[MAX_DEVICES];
 764         int64_t target_ts;
 765         int returned_events;
 766         int event_count;
 767
 768         /* Get one or more events from our collection of sensors */
 769
 770 return_available_sensor_reports:
 771
 772         returned_events = 0;
 773
 774         /* Check our sensor collection for available reports */
 775         for (s=0; s<sensor_count && returned_events < count; s++)
 776                 if (sensor_info[s].report_pending) {
 777                         event_count = 0;
 778                         /* Lower flag */
 779                         sensor_info[s].report_pending = 0;
 780
 781                         /* Report this event if it looks OK */
 782                         event_count = propagate_sensor_report(s, &data[returned_events]);
 783
 784                         /* Duplicate only if both cal & uncal are active */
 785                         if (sensor_catalog[sensor_info[s].catalog_index].type == SENSOR_TYPE_GYROSCOPE &&
 786                                         sensor_info[s].pair_idx && sensor_info[sensor_info[s].pair_idx].enable_count != 0) {
 787                                         struct gyro_cal* gyro_data = (struct gyro_cal*) sensor_info[s].cal_data;
 788
 789                                         memcpy(&data[returned_events + event_count], &data[returned_events],
 790                                                         sizeof(struct sensors_event_t) * event_count);
 791                                         for (i = 0; i < event_count; i++) {
 792                                                 data[returned_events + i].type = SENSOR_TYPE_GYROSCOPE_UNCALIBRATED;
 793                                                 data[returned_events + i].sensor = sensor_info[s].pair_idx;
 794
 795                                                 data[returned_events + i].data[0] = data[returned_events + i].data[0] + gyro_data->bias[0];
 796                                                 data[returned_events + i].data[1] = data[returned_events + i].data[1] + gyro_data->bias[1];
 797                                                 data[returned_events + i].data[2] = data[returned_events + i].data[2] + gyro_data->bias[2];
 798
 799                                                 data[returned_events + i].uncalibrated_gyro.bias[0] = gyro_data->bias[0];
 800                                                 data[returned_events + i].uncalibrated_gyro.bias[1] = gyro_data->bias[1];
 801                                                 data[returned_events + i].uncalibrated_gyro.bias[2] = gyro_data->bias[2];
 802                                         }
 803                                         event_count <<= 1;
 804                         }
 805                         sensor_info[sensor_info[s].pair_idx].report_pending = 0;
 806                         returned_events += event_count;
 807                         /*
 808                          * If the sample was deemed invalid or unreportable,
 809                          * e.g. had the same value as the previously reported
 810                          * value for a 'on change' sensor, silently drop it.
 811                          */
 812                 }
 813
 814         if (returned_events)
 815                 return returned_events;
 816
 817 await_event:
 818
 819         ALOGV("Awaiting sensor data\n");
 820
 821         nfds = epoll_wait(poll_fd, ev, MAX_DEVICES, -1);
 822
 823         if (nfds == -1) {
 824                 ALOGI("epoll_wait returned -1 (%s)\n", strerror(errno));
 825                 goto await_event;
 826         }
 827
 828         ALOGV("%d fds signalled\n", nfds);
 829
 830         /* For each of the signalled sources */
 831         for (i=0; i<nfds; i++)
 832                 if (ev[i].events == EPOLLIN)
 833                         switch (ev[i].data.u32) {
 834                                 case 0 ... MAX_DEVICES-1:
 835                                         /* Read report from iio char dev fd */
 836                                         integrate_device_report(ev[i].data.u32);
 837                                         break;
 838
 839                                 case THREAD_REPORT_TAG_BASE ...
 840                                      THREAD_REPORT_TAG_BASE + MAX_SENSORS-1:
 841                                         /* Get report from acquisition thread */
 842                                         integrate_thread_report(ev[i].data.u32);
 843                                         break;
 844
 845                                 default:
 846                                         ALOGW("Unexpected event source!\n");
 847                                         break;
 848                         }
 849
 850         goto return_available_sensor_reports;
 851 }
 852
 853
 854 int sensor_set_delay(int s, int64_t ns)
 855 {
 856         /* Set the rate at which a specific sensor should report events */
 857
 858         /* See Android sensors.h for indication on sensor trigger modes */
 859
 860         char sysfs_path[PATH_MAX];
 861         char avail_sysfs_path[PATH_MAX];
 862         int dev_num             =       sensor_info[s].dev_num;
 863         int i                   =       sensor_info[s].catalog_index;
 864         const char *prefix      =       sensor_catalog[i].tag;
 865         float new_sampling_rate; /* Granted sampling rate after arbitration   */
 866         float cur_sampling_rate; /* Currently used sampling rate              */
 867         int per_sensor_sampling_rate;
 868         int per_device_sampling_rate;
 869         float max_supported_rate = 0;
 870         char freqs_buf[100];
 871         char* cursor;
 872         int n;
 873         float sr;
 874
 875         if (!ns) {
 876                 ALOGE("Rejecting zero delay request on sensor %d\n", s);
 877                 return -EINVAL;
 878         }
 879
 880         new_sampling_rate = 1000000000LL/ns;
 881
 882         /*
 883          * Artificially limit ourselves to 1 Hz or higher. This is mostly to
 884          * avoid setting up the stage for divisions by zero.
 885          */
 886         if (new_sampling_rate < 1)
 887                 new_sampling_rate = 1;
 888
 889         sensor_info[s].sampling_rate = new_sampling_rate;
 890
 891         /* If we're dealing with a poll-mode sensor */
 892         if (!sensor_info[s].num_channels) {
 893                 /* Interrupt current sleep so the new sampling gets used */
 894                 pthread_cond_signal(&thread_release_cond[s]);
 895                 return 0;
 896         }
 897
 898         sprintf(sysfs_path, SENSOR_SAMPLING_PATH, dev_num, prefix);
 899
 900         if (sysfs_read_float(sysfs_path, &cur_sampling_rate) != -1) {
 901                 per_sensor_sampling_rate = 1;
 902                 per_device_sampling_rate = 0;
 903         } else {
 904                 per_sensor_sampling_rate = 0;
 905
 906                 sprintf(sysfs_path, DEVICE_SAMPLING_PATH, dev_num);
 907
 908                 if (sysfs_read_float(sysfs_path, &cur_sampling_rate) != -1)
 909                         per_device_sampling_rate = 1;
 910                 else
 911                         per_device_sampling_rate = 0;
 912         }
 913
 914         if (!per_sensor_sampling_rate && !per_device_sampling_rate) {
 915                 ALOGE("No way to adjust sampling rate on sensor %d\n", s);
 916                 return -ENOSYS;
 917         }
 918
 919         /* Coordinate with others active sensors on the same device, if any */
 920         if (per_device_sampling_rate)
 921                 for (n=0; n<sensor_count; n++)
 922                         if (n != s && sensor_info[n].dev_num == dev_num &&
 923                             sensor_info[n].num_channels &&
 924                             sensor_info[n].enable_count &&
 925                             sensor_info[n].sampling_rate > new_sampling_rate)
 926                                 new_sampling_rate= sensor_info[n].sampling_rate;
 927
 928         /* Check if we have contraints on allowed sampling rates */
 929
 930         sprintf(avail_sysfs_path, DEVICE_AVAIL_FREQ_PATH, dev_num);
 931
 932         if (sysfs_read_str(avail_sysfs_path, freqs_buf, sizeof(freqs_buf)) > 0){
 933                 cursor = freqs_buf;
 934
 935                 /* Decode allowed sampling rates string, ex: "10 20 50 100" */
 936
 937                 /* While we're not at the end of the string */
 938                 while (*cursor && cursor[0]) {
 939
 940                         /* Decode a single value */
 941                         sr = strtod(cursor, NULL);
 942
 943                         if (sr > max_supported_rate)
 944                                 max_supported_rate = sr;
 945
 946                         /* If this matches the selected rate, we're happy */
 947                         if (new_sampling_rate == sr)
 948                                 break;
 949
 950                         /*
 951                          * If we reached a higher value than the desired rate,
 952                          * adjust selected rate so it matches the first higher
 953                          * available one and stop parsing - this makes the
 954                          * assumption that rates are sorted by increasing value
 955                          * in the allowed frequencies string.
 956                          */
 957                         if (sr > new_sampling_rate) {
 958                                 new_sampling_rate = sr;
 959                                 break;
 960                         }
 961
 962                         /* Skip digits */
 963                         while (cursor[0] && !isspace(cursor[0]))
 964                                 cursor++;
 965
 966                         /* Skip spaces */
 967                         while (cursor[0] && isspace(cursor[0]))
 968                                         cursor++;
 969                 }
 970         }
 971
 972
 973         if (max_supported_rate &&
 974                 new_sampling_rate > max_supported_rate) {
 975                 new_sampling_rate = max_supported_rate;
 976         }
 977
 978
 979         /* If the desired rate is already active we're all set */
 980         if (new_sampling_rate == cur_sampling_rate)
 981                 return 0;
 982
 983         ALOGI("Sensor %d sampling rate set to %g\n", s, new_sampling_rate);
 984
 985         if (trig_sensors_per_dev[dev_num])
 986                 enable_buffer(dev_num, 0);
 987
 988         sysfs_write_float(sysfs_path, new_sampling_rate);
 989
 990         if (trig_sensors_per_dev[dev_num])
 991                 enable_buffer(dev_num, 1);
 992
 993         return 0;
 994 }
 995
 996
 997 int allocate_control_data (void)
 998 {
 999         int i;
1000         struct epoll_event ev = {0};
1001
1002         for (i=0; i<MAX_DEVICES; i++)
1003                 device_fd[i] = -1;
1004
1005         poll_fd = epoll_create(MAX_DEVICES);
1006
1007         if (poll_fd == -1) {
1008                 ALOGE("Can't create epoll instance for iio sensors!\n");
1009                 return -1;
1010         }
1011
1012         return poll_fd;
1013 }
1014
1015
1016 void delete_control_data (void)
1017 {
1018 }