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