control.c

   1 /*
   2  * Copyright (C) 2014 Intel Corporation.
   3  */
   4
   5 #include <fcntl.h>
   6 #include <sys/epoll.h>
   7 #include <math.h>
   8 #include <utils/Log.h>
   9 #include <hardware/sensors.h>
  10 #include "control.h"
  11 #include "enumeration.h"
  12 #include "utils.h"
  13
  14 /* Currently active sensors count, per device */
  15 static int poll_sensors_per_dev[MAX_DEVICES];   /* poll-mode sensors */
  16 static int trig_sensors_per_dev[MAX_DEVICES];   /* trigger, event based */
  17
  18 static int device_fd[MAX_DEVICES];   /* fd on the /dev/iio:deviceX file */
  19
  20 static int poll_fd; /* epoll instance covering all enabled sensors */
  21
  22 /* Timestamp for the moment when we last exited a poll operation */
  23 static int64_t last_poll_exit_ts;
  24
  25 /* Cap the time between poll operations to this, to counter runaway polls */
  26 #define POLL_MIN_INTERVAL 10000 /* uS */
  27
  28 static int active_poll_sensors; /* Number of enabled poll-mode sensors */
  29
  30
  31 static int enable_buffer(int dev_num, int enabled)
  32 {
  33         char sysfs_path[PATH_MAX];
  34
  35         sprintf(sysfs_path, ENABLE_PATH, dev_num);
  36
  37         /* Low level, non-multiplexed, enable/disable routine */
  38         return sysfs_write_int(sysfs_path, enabled);
  39 }
  40
  41
  42 static int setup_trigger(int dev_num, const char* trigger_val)
  43 {
  44         char sysfs_path[PATH_MAX];
  45
  46         sprintf(sysfs_path, TRIGGER_PATH, dev_num);
  47
  48         return sysfs_write_str(sysfs_path, trigger_val);
  49 }
  50
  51
  52 static void refresh_sensor_report_maps(int dev_num)
  53 {
  54         /*
  55          * Read sysfs files from a iio device's scan_element directory, and
  56          * build a couple of tables from that data. These tables will tell, for
  57          * each sensor, where to gather relevant data in a device report, i.e.
  58          * the structure that we read from the /dev/iio:deviceX file in order to
  59          * sensor report, itself being the data that we return to Android when a
  60          * sensor poll completes. The mapping should be straightforward in the
  61          * case where we have a single sensor active per iio device but, this is
  62          * not the general case. In general several sensors can be handled
  63          * through a single iio device, and the _en, _index and _type syfs
  64          * entries all concur to paint a picture of what the structure of the
  65          * device report is.
  66          */
  67
  68         int s;
  69         int c;
  70         int n;
  71         int i;
  72         int ch_enabled;
  73         int ch_index;
  74         char* ch_spec;
  75         char spec_buf[MAX_TYPE_SPEC_LEN];
  76         struct datum_info_t* ch_info;
  77         int size;
  78         char sysfs_path[PATH_MAX];
  79         int active_channels;
  80         int offset;
  81         int channel_count;
  82         int channel_size_from_index[MAX_SENSORS * MAX_CHANNELS] = { 0 };
  83         int sensor_handle_from_index[MAX_SENSORS * MAX_CHANNELS] = { 0 };
  84         int channel_number_from_index[MAX_SENSORS * MAX_CHANNELS] = { 0 };
  85
  86         active_channels = 0;
  87
  88         /* For each sensor that is linked to this device */
  89         for (s=0; s<sensor_count; s++) {
  90                 if (sensor_info[s].dev_num != dev_num)
  91                         continue;
  92
  93                 i = sensor_info[s].catalog_index;
  94
  95                 /* Read channel status through syfs attributes */
  96                 for (c=0; c<sensor_info[s].num_channels; c++) {
  97
  98                         /* Read _en file */
  99                         sprintf(sysfs_path, CHANNEL_PATH "%s",
 100                                 sensor_info[s].dev_num,
 101                                 sensor_catalog[i].channel[c].en_path);
 102
 103                         n = sysfs_read_int(sysfs_path, &ch_enabled);
 104
 105                         if (n == -1) {
 106                                 ALOGW(  "Failed to read _en flag: %s\n",
 107                                 sysfs_path);
 108                                 continue;
 109                         }
 110
 111                         if (!ch_enabled != 1) {
 112                                 sensor_info[s].channel[c].size = 0;
 113                         }
 114
 115                         /* Read _type file */
 116                         sprintf(sysfs_path, CHANNEL_PATH "%s",
 117                                 sensor_info[s].dev_num,
 118                                 sensor_catalog[i].channel[c].type_path);
 119
 120                         n = sysfs_read_str(sysfs_path, spec_buf,
 121                                                 sizeof(spec_buf));
 122
 123                         if (n == -1) {
 124                                         ALOGW(  "Failed to read type: %s\n",
 125                                         sysfs_path);
 126                                         continue;
 127                                 }
 128
 129                         ch_spec = sensor_info[s].channel[c].type_spec;
 130
 131                         memcpy(ch_spec, spec_buf, sizeof(spec_buf));
 132
 133                         ch_info = &sensor_info[s].channel[c].type_info;
 134
 135                         size = decode_type_spec(ch_spec, ch_info);
 136
 137                         /* Read _index file */
 138                         sprintf(sysfs_path, CHANNEL_PATH "%s",
 139                                 sensor_info[s].dev_num,
 140                                 sensor_catalog[i].channel[c].index_path);
 141
 142                         n = sysfs_read_int(sysfs_path, &ch_index);
 143
 144                         if (n == -1) {
 145                                         ALOGW(  "Failed to read index: %s\n",
 146                                                 sysfs_path);
 147                                         continue;
 148                                 }
 149
 150                         if (ch_index >= MAX_SENSORS) {
 151                                 ALOGE("Index out of bounds!: %s\n", sysfs_path);
 152                                 continue;
 153                         }
 154
 155                         /* Record what this index is about */
 156
 157                         sensor_handle_from_index [ch_index] = s;
 158                         channel_number_from_index[ch_index] = c;
 159                         channel_size_from_index  [ch_index] = size;
 160
 161                         active_channels++;
 162                 }
 163         }
 164
 165         ALOGI("Found %d enabled channels for iio device %d\n", active_channels,
 166                 dev_num);
 167
 168         /*
 169          * Now that we know which channels are enabled, their sizes and their
 170          * ordering, update channels offsets within device report. Note: there
 171          * is a possibility that several sensors share the same index, with
 172          * their data fields being isolated by masking and shifting as specified
 173          * through the real bits and shift values in type attributes. This case
 174          * is not currently supported. Also, the code below assumes no hole in
 175          * the sequence of indices, so it is dependent on discovery of all
 176          * sensors.
 177          */
 178          offset = 0;
 179          for (i=0; i<MAX_SENSORS * MAX_CHANNELS; i++) {
 180                 s =     sensor_handle_from_index[i];
 181                 c =     channel_number_from_index[i];
 182                 size =  channel_size_from_index[i];
 183
 184                 if (!size)
 185                         continue;
 186
 187                 ALOGI("S%d C%d : offset %d, size %d, type %s\n",
 188                       s, c, offset, size, sensor_info[s].channel[c].type_spec);
 189
 190                 sensor_info[s].channel[c].offset        = offset;
 191                 sensor_info[s].channel[c].size          = size;
 192
 193                 offset += size;
 194          }
 195 }
 196
 197
 198 int adjust_counters (int s, int enabled)
 199 {
 200         /*
 201          * Adjust counters based on sensor enable action. Return values are:
 202          * -1 if there's an inconsistency: abort action in this case
 203          *  0 if the operation was completed and we're all set
 204          *  1 if we toggled the state of the sensor and there's work left
 205          */
 206
 207         int dev_num = sensor_info[s].dev_num;
 208
 209         /* Refcount per sensor, in terms of enable count */
 210         if (enabled) {
 211                 ALOGI("Enabling sensor %d (iio device %d: %s)\n",
 212                         s, dev_num, sensor_info[s].internal_name);
 213
 214                 sensor_info[s].enable_count++;
 215
 216                 if (sensor_info[s].enable_count != 1)
 217                         return 0; /* The sensor was, and remains, in use */
 218         } else {
 219                 if (sensor_info[s].enable_count == 0)
 220                         return -1; /* Spurious disable call */
 221
 222                 ALOGI("Disabling sensor %d (iio device %d)\n", s, dev_num);
 223
 224                 sensor_info[s].enable_count--;
 225
 226                 if (sensor_info[s].enable_count > 0)
 227                         return 0; /* The sensor was, and remains, in use */
 228
 229                 /* Sensor disabled, clear up pending data */
 230
 231                 sensor_info[s].report_pending = 0;
 232                 memset(sensor_info[s].report_buffer, 0, MAX_SENSOR_REPORT_SIZE);
 233         }
 234
 235         /* We changed the state of a sensor - adjust per iio device counters */
 236
 237         /* If this is a regular event-driven sensor */
 238         if (sensor_info[s].num_channels) {
 239
 240                         if (enabled)
 241                                 trig_sensors_per_dev[dev_num]++;
 242                         else
 243                                 trig_sensors_per_dev[dev_num]--;
 244
 245                         return 1;
 246                 }
 247
 248         if (enabled) {
 249                 active_poll_sensors++;
 250                 poll_sensors_per_dev[dev_num]++;
 251                 return 1;
 252         }
 253
 254         active_poll_sensors--;
 255         poll_sensors_per_dev[dev_num]--;
 256         return 1;
 257 }
 258
 259
 260 int sensor_activate(int s, int enabled)
 261 {
 262         char sysfs_path[PATH_MAX];
 263         char device_name[PATH_MAX];
 264         char trigger_name[MAX_NAME_SIZE + 16];
 265         int c;
 266         struct epoll_event ev = {0};
 267         int dev_fd;
 268         int ret;
 269         int dev_num = sensor_info[s].dev_num;
 270         int i = sensor_info[s].catalog_index;
 271         int is_poll_sensor = !sensor_info[s].num_channels;
 272
 273         ret = adjust_counters(s, enabled);
 274
 275         /* If the operation was neutral in terms of state, we're done */
 276         if (ret <= 0)
 277                 return ret;
 278
 279         if (!is_poll_sensor) {
 280                 /* Changes have to be made while the buffer is turned off */
 281                 enable_buffer(dev_num, 0);
 282
 283                 /* Configure trigger */
 284                 switch (trig_sensors_per_dev[dev_num]) {
 285                         case 0:
 286                                 setup_trigger(dev_num, "none");
 287                                 break;
 288
 289                         case 1:
 290                                 sprintf(trigger_name, "%s-dev%d",
 291                                         sensor_info[s].internal_name, dev_num);
 292
 293                                 setup_trigger(dev_num, trigger_name);
 294                                 break;
 295
 296                         default:
 297                                 /* The trigger is already set */
 298                                 break;
 299                 }
 300
 301                 /*
 302                  * Turn channels associated to this sensor on or off, and update
 303                  * the channels maps for all sensors associated to this device.
 304                  */
 305                 for (c=0;c<sensor_info[s].num_channels; c++) {
 306                         sprintf(sysfs_path, CHANNEL_PATH "%s",
 307                                 sensor_info[s].dev_num,
 308                                 sensor_catalog[i].channel[c].en_path);
 309
 310                         sysfs_write_int(sysfs_path, enabled);
 311                 }
 312
 313                 /* If there's at least one sensor left */
 314                 if (trig_sensors_per_dev[dev_num]) {
 315                         refresh_sensor_report_maps(dev_num);
 316                         enable_buffer(dev_num, 1);
 317                 }
 318         }
 319
 320         /*
 321          * Make sure we have a fd on the character device ; conversely, close
 322          * the fd if no one is using associated sensor anymore. The assumption
 323          * here is that the underlying driver will power on the relevant
 324          * hardware block while someone hold a fd on the device.
 325          */
 326         dev_fd = device_fd[dev_num];
 327
 328         switch (poll_sensors_per_dev[dev_num] + trig_sensors_per_dev[dev_num]) {
 329
 330                 case 0:
 331                         if (dev_fd != -1) {
 332                                 /*
 333                                  * Stop watching this fd. This should be a no-op
 334                                  * in case this fd was not in the poll set.
 335                                  */
 336                                 epoll_ctl(poll_fd, EPOLL_CTL_DEL, dev_fd, NULL);
 337
 338                                 close(dev_fd);
 339                                 device_fd[dev_num] = -1;
 340                         }
 341                         return 0;
 342
 343                 case 1:
 344                         /* First enabled sensor on this iio device */
 345                         sprintf(device_name, DEV_FILE_PATH, dev_num);
 346                         dev_fd = open(device_name, O_RDONLY | O_NONBLOCK);
 347
 348                         device_fd[dev_num] = dev_fd;
 349
 350                         if (dev_fd == -1) {
 351                                 ALOGE("Could not open fd on %s (%s)\n",
 352                                       device_name, strerror(errno));
 353                                 adjust_counters(s, 0);
 354                                 return -1;
 355                         }
 356
 357                         ALOGV("Opened %s: fd=%d\n", device_name, dev_fd);
 358                         break;
 359
 360                 default:
 361                         break;
 362         }
 363
 364         if (!is_poll_sensor && trig_sensors_per_dev[dev_num] == 1) {
 365
 366                 /* Add this iio device fd to the set of watched fds */
 367                 ev.events = EPOLLIN;
 368                 ev.data.u32 = dev_num;
 369
 370                 ret = epoll_ctl(poll_fd, EPOLL_CTL_ADD, dev_fd, &ev);
 371
 372                 if (ret == -1) {
 373                         ALOGE("Failed adding %d to poll set (%s)\n", dev_fd,
 374                               strerror(errno));
 375                         return -1;
 376                 }
 377
 378                 /* Note: poll-mode fds are not readable */
 379         }
 380
 381         return 0;
 382 }
 383
 384
 385 static int integrate_device_report(int dev_num)
 386 {
 387         int len;
 388         int s,c;
 389         unsigned char buf[MAX_SENSOR_REPORT_SIZE * MAX_SENSORS] = { 0 };
 390         int sr_offset;
 391         unsigned char *target;
 392         unsigned char *source;
 393         int size;
 394         int expected_size = 0;
 395
 396         /* There's an incoming report on the specified fd */
 397
 398         if (dev_num < 0 || dev_num >= MAX_DEVICES ||
 399                 !trig_sensors_per_dev[dev_num]) {
 400                 ALOGE("Event reported on unexpected iio device %d\n", dev_num);
 401                 return -1;
 402         }
 403
 404         for (s=0; s<MAX_SENSORS; s++)
 405                 if (sensor_info[s].dev_num == dev_num)
 406                         for (c=0; c<sensor_info[s].num_channels; c++)
 407                                 expected_size += sensor_info[s].channel[c].size;
 408
 409         len = read(device_fd[dev_num], buf, expected_size);
 410
 411         if (len == -1) {
 412                 ALOGE("Could not read report from iio device %d (%s)\n",
 413                       dev_num, strerror(errno));
 414                 return -1;
 415         }
 416
 417         ALOGV("Read %d bytes from iio device %d\n", len, dev_num);
 418
 419         for (s=0; s<MAX_SENSORS; s++)
 420                 if (sensor_info[s].dev_num == dev_num) {
 421                         sr_offset = 0;
 422
 423                         /* Copy data from device to sensor report buffer */
 424                         for (c=0; c<sensor_info[s].num_channels; c++) {
 425
 426                                 target = sensor_info[s].report_buffer +
 427                                         sr_offset;
 428
 429                                 source = buf + sensor_info[s].channel[c].offset;
 430
 431                                 size = sensor_info[s].channel[c].size;
 432
 433                                 memcpy(target, source, size);
 434
 435                                 sr_offset += size;
 436                         }
 437
 438                         if (sensor_info[s].enable_count) {
 439                                 ALOGV("Sensor %d report available (%d bytes)\n",
 440                                       s, sr_offset);
 441
 442                                 sensor_info[s].report_pending = 1;
 443                         }
 444                 }
 445
 446         return 0;
 447 }
 448
 449
 450 static float acquire_immediate_value(int s, int c)
 451 {
 452         char sysfs_path[PATH_MAX];
 453         float val;
 454         int ret;
 455         int dev_num = sensor_info[s].dev_num;
 456         int i = sensor_info[s].catalog_index;
 457         const char* raw_path = sensor_catalog[i].channel[c].raw_path;
 458         const char* input_path = sensor_catalog[i].channel[c].input_path;
 459         float scale = sensor_info[s].scale;
 460         float offset = sensor_info[s].offset;
 461
 462         /* Acquire a sample value for sensor s / channel c through sysfs */
 463
 464         if (input_path[0]) {
 465                 sprintf(sysfs_path, BASE_PATH "%s", dev_num, input_path);
 466                 ret = sysfs_read_float(sysfs_path, &val);
 467
 468                 if (!ret) {
 469                         return val;
 470                 }
 471         };
 472
 473         if (!raw_path[0])
 474                 return 0;
 475
 476         sprintf(sysfs_path, BASE_PATH "%s", dev_num, raw_path);
 477         ret = sysfs_read_float(sysfs_path, &val);
 478
 479         if (ret == -1)
 480                 return 0;
 481
 482         return (val + offset) * scale;
 483 }
 484
 485
 486 static void propagate_sensor_report(int s, struct sensors_event_t* data)
 487 {
 488         /* There's a sensor report pending for this sensor ; transmit it */
 489
 490         int catalog_index = sensor_info[s].catalog_index;
 491         int sensor_type = sensor_catalog[catalog_index].type;
 492         int num_fields;
 493         int c;
 494         unsigned char* current_sample;
 495         int sample_size;
 496         struct datum_info_t* sample_type;
 497         int64_t s64;
 498         float val;
 499
 500         memset(data, 0, sizeof(sensors_event_t));
 501
 502         data->version = sizeof(sensors_event_t);
 503         data->sensor = s;
 504         data->type = sensor_type;
 505         data->timestamp = get_timestamp();
 506
 507         switch (sensor_type) {
 508                 case SENSOR_TYPE_ACCELEROMETER:         /* m/s^2        */
 509                 case SENSOR_TYPE_MAGNETIC_FIELD:        /* micro-tesla  */
 510                 case SENSOR_TYPE_ORIENTATION:           /* degrees      */
 511                 case SENSOR_TYPE_GYROSCOPE:             /* radians/s    */
 512                         num_fields = 3;
 513                         break;
 514
 515                 case SENSOR_TYPE_LIGHT:                 /* SI lux units */
 516                 case SENSOR_TYPE_AMBIENT_TEMPERATURE:   /* °C          */
 517                 case SENSOR_TYPE_TEMPERATURE:           /* °C          */
 518                 case SENSOR_TYPE_PROXIMITY:             /* centimeters  */
 519                 case SENSOR_TYPE_PRESSURE:              /* hecto-pascal */
 520                 case SENSOR_TYPE_RELATIVE_HUMIDITY:     /* percent */
 521                         num_fields = 1;
 522                         break;
 523
 524                 case SENSOR_TYPE_ROTATION_VECTOR:
 525                         num_fields = 4;
 526                         break;
 527
 528                 case SENSOR_TYPE_DEVICE_PRIVATE_BASE:   /* hidden for now */
 529                         num_fields = 0;
 530                         break;
 531
 532                 default:
 533                         ALOGE("Unknown sensor type!\n");
 534                         num_fields = 0;
 535                         break;
 536         }
 537
 538         ALOGV("Sample on sensor %d (type %d):\n", s, sensor_type);
 539
 540         /* If we're dealing with a poll-mode sensor */
 541         if (!sensor_info[s].num_channels) {
 542
 543                 /* Read values through sysfs rather than from a report buffer */
 544                 for (c=0; c<num_fields; c++) {
 545                         val = acquire_immediate_value(s, c);
 546
 547                         data->data[c] = transform_sample(sensor_type, c, val);
 548
 549                         ALOGV("\tfield %d: %f\n", c, data->data[c]);
 550                 }
 551                 return;
 552         }
 553
 554         /* Convert the data into the expected Android-level format */
 555
 556         current_sample = sensor_info[s].report_buffer;
 557
 558         for (c=0; c<num_fields; c++) {
 559                 sample_size     =  sensor_info[s].channel[c].size;
 560                 sample_type     = &sensor_info[s].channel[c].type_info;
 561
 562                 s64 = sample_as_int64(current_sample, sample_type);
 563
 564                 val = (sensor_info[s].offset + s64) * sensor_info[s].scale;
 565
 566                 data->data[c] = transform_sample(sensor_type, c, val);
 567
 568                 ALOGV("\tfield %d: %f\n", c, data->data[c]);
 569                 current_sample += sample_size;
 570         }
 571 }
 572
 573
 574 static int get_poll_time (void)
 575 {
 576         if (!active_poll_sensors)
 577                 return -1;      /* Infinite wait */
 578
 579         return 100;     /* ms ... this needs to be dynamic */
 580 }
 581
 582
 583 int sensor_poll(struct sensors_event_t* data, int count)
 584 {
 585         int s;
 586         int i;
 587         int nfds;
 588         int delta;
 589         struct epoll_event ev[MAX_DEVICES];
 590
 591         /* Get one or more events from our collection of sensors */
 592
 593 return_first_available_sensor_report:
 594
 595         /* If there's at least one available report */
 596         for (s=0; s<sensor_count; s++)
 597                 if (sensor_info[s].report_pending) {
 598
 599                         /* Return that up */
 600                         propagate_sensor_report(s, data);
 601                         sensor_info[s].report_pending = 0;
 602                         ALOGV("Report on sensor %d\n", s);
 603                         return 1;
 604                 }
 605
 606         /* Keep a minimum time interval between poll operations */
 607         delta = (get_timestamp() - last_poll_exit_ts)/1000;
 608
 609         if (delta > 0 && delta < POLL_MIN_INTERVAL)
 610                 usleep(POLL_MIN_INTERVAL - delta);
 611
 612         ALOGV("Awaiting sensor data\n");
 613
 614         nfds = epoll_wait(poll_fd, ev, MAX_DEVICES, get_poll_time());
 615
 616         last_poll_exit_ts = get_timestamp();
 617
 618         ALOGV("%d fds signalled\n", nfds);
 619
 620         /* For each of the devices for which a report is available */
 621         for (i=0; i<nfds; i++)
 622                 if (ev[i].events == EPOLLIN)
 623                         /* Read report */
 624                         integrate_device_report(ev[i].data.u32);
 625
 626         /* It's a good time to invalidate poll-mode sensor values */
 627         if (active_poll_sensors)
 628                 for (s=0; s<sensor_count; s++)
 629                         if (sensor_info[s].enable_count &&
 630                                 !sensor_info[s].num_channels)
 631                                         sensor_info[s].report_pending = 1;
 632
 633         goto return_first_available_sensor_report;
 634 }
 635
 636
 637 int sensor_set_delay(int handle, int64_t ns)
 638 {
 639         /* Set the rate at which a specific sensor should report events */
 640         /* Continuous reports: accelerometer, gyroscope */
 641         /* On change with minimum delay between events: ALS, proximity */
 642         /* See sensors.h for indication on sensor trigger modes */
 643         return -1;
 644 }
 645
 646
 647 int allocate_control_data (void)
 648 {
 649         int i;
 650
 651         for (i=0; i<MAX_DEVICES; i++)
 652                 device_fd[i] = -1;
 653
 654         poll_fd = epoll_create(MAX_DEVICES);
 655
 656         if (poll_fd == -1) {
 657                 ALOGE("Can't create epoll instance for iio sensors!\n");
 658         }
 659
 660         return poll_fd;
 661 }
 662
 663
 664 void delete_control_data (void)
 665 {
 666 }