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                         /* Record what this index is about */
 151
 152                         sensor_handle_from_index [ch_index] = s;
 153                         channel_number_from_index[ch_index] = c;
 154                         channel_size_from_index  [ch_index] = size;
 155
 156                         active_channels++;
 157                 }
 158         }
 159
 160         ALOGI("Found %d enabled channels for iio device %d\n", active_channels,
 161                 dev_num);
 162
 163         /*
 164          * Now that we know which channels are enabled, their sizes and their
 165          * ordering, update channels offsets within device report. Note: there
 166          * is a possibility that several sensors share the same index, with
 167          * their data fields being isolated by masking and shifting as specified
 168          * through the real bits and shift values in type attributes. This case
 169          * is not currently supported. Also, the code below assumes no hole in
 170          * the sequence of indices, so it is dependent on discovery of all
 171          * sensors.
 172          */
 173          offset = 0;
 174          for (i=0; i<MAX_SENSORS * MAX_CHANNELS; i++) {
 175                 s =     sensor_handle_from_index[i];
 176                 c =     channel_number_from_index[i];
 177                 size =  channel_size_from_index[i];
 178
 179                 if (!size)
 180                         continue;
 181
 182                 ALOGI("S%d C%d : offset %d, size %d, type %s\n",
 183                       s, c, offset, size, sensor_info[s].channel[c].type_spec);
 184
 185                 sensor_info[s].channel[c].offset        = offset;
 186                 sensor_info[s].channel[c].size          = size;
 187
 188                 offset += size;
 189          }
 190 }
 191
 192
 193 int adjust_counters (int s, int enabled)
 194 {
 195         /*
 196          * Adjust counters based on sensor enable action. Return values are:
 197          * -1 if there's an inconsistency: abort action in this case
 198          *  0 if the operation was completed and we're all set
 199          *  1 if we toggled the state of the sensor and there's work left
 200          */
 201
 202         int dev_num = sensor_info[s].dev_num;
 203
 204         /* Refcount per sensor, in terms of enable count */
 205         if (enabled) {
 206                 ALOGI("Enabling sensor %d (iio device %d: %s)\n",
 207                         s, dev_num, sensor_info[s].internal_name);
 208
 209                 sensor_info[s].enable_count++;
 210
 211                 if (sensor_info[s].enable_count != 1)
 212                         return 0; /* The sensor was, and remains, in use */
 213         } else {
 214                 if (sensor_info[s].enable_count == 0)
 215                         return -1; /* Spurious disable call */
 216
 217                 ALOGI("Disabling sensor %d (iio device %d)\n", s, dev_num);
 218
 219                 sensor_info[s].enable_count--;
 220
 221                 if (sensor_info[s].enable_count > 0)
 222                         return 0; /* The sensor was, and remains, in use */
 223
 224                 /* Sensor disabled, clear up pending data */
 225
 226                 sensor_info[s].report_pending = 0;
 227                 memset(sensor_info[s].report_buffer, 0, MAX_SENSOR_REPORT_SIZE);
 228         }
 229
 230         /* We changed the state of a sensor - adjust per iio device counters */
 231
 232         /* If this is a regular event-driven sensor */
 233         if (sensor_info[s].num_channels) {
 234
 235                         if (enabled)
 236                                 trig_sensors_per_dev[dev_num]++;
 237                         else
 238                                 trig_sensors_per_dev[dev_num]--;
 239
 240                         return 1;
 241                 }
 242
 243         if (enabled) {
 244                 active_poll_sensors++;
 245                 poll_sensors_per_dev[dev_num]++;
 246                 return 1;
 247         }
 248
 249         active_poll_sensors--;
 250         poll_sensors_per_dev[dev_num]--;
 251         return 1;
 252 }
 253
 254
 255 int sensor_activate(int s, int enabled)
 256 {
 257         char sysfs_path[PATH_MAX];
 258         char device_name[PATH_MAX];
 259         char trigger_name[MAX_NAME_SIZE + 16];
 260         int c;
 261         struct epoll_event ev = {0};
 262         int dev_fd;
 263         int ret;
 264         int dev_num = sensor_info[s].dev_num;
 265         int i = sensor_info[s].catalog_index;
 266         int is_poll_sensor = !sensor_info[s].num_channels;
 267
 268         ret = adjust_counters(s, enabled);
 269
 270         /* If the operation was neutral in terms of state, we're done */
 271         if (ret <= 0)
 272                 return ret;
 273
 274         if (!is_poll_sensor) {
 275                 /* Changes have to be made while the buffer is turned off */
 276                 enable_buffer(dev_num, 0);
 277
 278                 /* Configure trigger */
 279                 switch (trig_sensors_per_dev[dev_num]) {
 280                         case 0:
 281                                 setup_trigger(dev_num, "none");
 282                                 break;
 283
 284                         case 1:
 285                                 sprintf(trigger_name, "%s-dev%d",
 286                                         sensor_info[s].internal_name, dev_num);
 287
 288                                 setup_trigger(dev_num, trigger_name);
 289                                 break;
 290
 291                         default:
 292                                 /* The trigger is already set */
 293                                 break;
 294                 }
 295
 296                 /*
 297                  * Turn channels associated to this sensor on or off, and update
 298                  * the channels maps for all sensors associated to this device.
 299                  */
 300                 for (c=0;c<sensor_info[s].num_channels; c++) {
 301                         sprintf(sysfs_path, CHANNEL_PATH "%s",
 302                                 sensor_info[s].dev_num,
 303                                 sensor_catalog[i].channel[c].en_path);
 304
 305                         sysfs_write_int(sysfs_path, enabled);
 306                 }
 307
 308                 /* If there's at least one sensor left */
 309                 if (trig_sensors_per_dev[dev_num]) {
 310                         refresh_sensor_report_maps(dev_num);
 311                         enable_buffer(dev_num, 1);
 312                 }
 313         }
 314
 315         /*
 316          * Make sure we have a fd on the character device ; conversely, close
 317          * the fd if no one is using associated sensor anymore. The assumption
 318          * here is that the underlying driver will power on the relevant
 319          * hardware block while someone hold a fd on the device.
 320          */
 321         dev_fd = device_fd[dev_num];
 322
 323         switch (poll_sensors_per_dev[dev_num] + trig_sensors_per_dev[dev_num]) {
 324
 325                 case 0:
 326                         if (dev_fd != -1) {
 327                                 /*
 328                                  * Stop watching this fd. This should be a no-op
 329                                  * in case this fd was not in the poll set.
 330                                  */
 331                                 epoll_ctl(poll_fd, EPOLL_CTL_DEL, dev_fd, NULL);
 332
 333                                 close(dev_fd);
 334                                 device_fd[dev_num] = -1;
 335                         }
 336                         return 0;
 337
 338                 case 1:
 339                         /* First enabled sensor on this iio device */
 340                         sprintf(device_name, DEV_FILE_PATH, dev_num);
 341                         dev_fd = open(device_name, O_RDONLY | O_NONBLOCK);
 342
 343                         if (dev_fd == -1) {
 344                                 ALOGE("Could not open fd on %s (%s)\n",
 345                                       device_name, strerror(errno));
 346                                 return -1;
 347                         }
 348
 349                         ALOGV("Opened %s: fd=%d\n", device_name, dev_fd);
 350
 351                         device_fd[dev_num] = dev_fd;
 352                         break;
 353
 354                 default:
 355                         break;
 356         }
 357
 358         if (!is_poll_sensor && trig_sensors_per_dev[dev_num] == 1) {
 359
 360                 /* Add this iio device fd to the set of watched fds */
 361                 ev.events = EPOLLIN;
 362                 ev.data.u32 = dev_num;
 363
 364                 ret = epoll_ctl(poll_fd, EPOLL_CTL_ADD, dev_fd, &ev);
 365
 366                 if (ret == -1) {
 367                         ALOGE("Failed adding %d to poll set (%s)\n", dev_fd,
 368                               strerror(errno));
 369                         return -1;
 370                 }
 371
 372                 /* Note: poll-mode fds are not readable */
 373         }
 374
 375         return 0;
 376 }
 377
 378
 379 static int integrate_device_report(int dev_num)
 380 {
 381         int len;
 382         int s,c;
 383         unsigned char buf[MAX_SENSOR_REPORT_SIZE * MAX_SENSORS] = { 0 };
 384         int dr_offset;
 385         int sr_offset;
 386         unsigned char *target;
 387         unsigned char *source;
 388         int size;
 389         int expected_size = 0;
 390
 391         if (dev_num < 0 || dev_num >= MAX_DEVICES)
 392                 return -1;
 393
 394         /* There's an incoming report on the specified fd */
 395
 396         for (s=0; s<MAX_SENSORS; s++)
 397                 if (sensor_info[s].enable_count &&
 398                     sensor_info[s].dev_num == dev_num)
 399                         for (c=0; c<sensor_info[s].num_channels; c++)
 400                                 expected_size += sensor_info[s].channel[c].size;
 401
 402         if (expected_size == 0)
 403                 return 0;
 404
 405         len = read(device_fd[dev_num], buf, expected_size);
 406
 407         if (len == -1) {
 408                 ALOGE("Could not read report from iio device %d (%s)\n",
 409                       dev_num, strerror(errno));
 410                 return -1;
 411         }
 412
 413         ALOGV("Read %d bytes from iio device %d\n", len, dev_num);
 414
 415         dr_offset = 0;
 416
 417         for (s=0; s<MAX_SENSORS; s++)
 418                 if (sensor_info[s].enable_count &&
 419                     sensor_info[s].dev_num == dev_num) {
 420                         sr_offset = 0;
 421
 422                         /* Copy data from device to sensor report buffer */
 423                         for (c=0; c<sensor_info[s].num_channels; c++) {
 424
 425                                 target = sensor_info[s].report_buffer +
 426                                         sr_offset;
 427
 428                                 source = buf + sensor_info[s].channel[c].offset;
 429
 430                                 size = sensor_info[s].channel[c].size;
 431
 432                                 memcpy(target, source, size);
 433
 434                                 sr_offset += size;
 435                         }
 436
 437                         ALOGV("Sensor %d report available (%d bytes)\n",
 438                                 s, sr_offset);
 439
 440                         sensor_info[s].report_pending = 1;
 441                 }
 442
 443         return 0;
 444 }
 445
 446
 447 static float acquire_immediate_value(int s, int c)
 448 {
 449         char sysfs_path[PATH_MAX];
 450         float val;
 451         int ret;
 452         int dev_num = sensor_info[s].dev_num;
 453         int i = sensor_info[s].catalog_index;
 454         const char* raw_path = sensor_catalog[i].channel[c].raw_path;
 455         const char* input_path = sensor_catalog[i].channel[c].input_path;
 456         float scale = sensor_info[s].scale;
 457         float offset = sensor_info[s].offset;
 458
 459         /* Acquire a sample value for sensor s / channel c through sysfs */
 460
 461         if (input_path[0]) {
 462                 sprintf(sysfs_path, BASE_PATH "%s", dev_num, input_path);
 463                 ret = sysfs_read_float(sysfs_path, &val);
 464
 465                 if (!ret) {
 466                         return val;
 467                 }
 468         };
 469
 470         if (!raw_path[0])
 471                 return 0;
 472
 473         sprintf(sysfs_path, BASE_PATH "%s", dev_num, raw_path);
 474         ret = sysfs_read_float(sysfs_path, &val);
 475
 476         if (ret == -1)
 477                 return 0;
 478
 479         return val;
 480 }
 481
 482
 483 static void propagate_sensor_report(int s, struct sensors_event_t* data)
 484 {
 485         /* There's a sensor report pending for this sensor ; transmit it */
 486
 487         int catalog_index = sensor_info[s].catalog_index;
 488         int sensor_type = sensor_catalog[catalog_index].type;
 489         int num_fields;
 490         int c;
 491         unsigned char* current_sample;
 492         int sample_size;
 493         struct datum_info_t* sample_type;
 494         int64_t s64;
 495         float val;
 496
 497         memset(data, 0, sizeof(sensors_event_t));
 498
 499         data->version = sizeof(sensors_event_t);
 500         data->sensor = s;
 501         data->type = sensor_type;
 502         data->timestamp = get_timestamp();
 503
 504         switch (sensor_type) {
 505                 case SENSOR_TYPE_ACCELEROMETER:         /* m/s^2        */
 506                 case SENSOR_TYPE_MAGNETIC_FIELD:        /* micro-tesla  */
 507                 case SENSOR_TYPE_ORIENTATION:           /* degrees      */
 508                 case SENSOR_TYPE_GYROSCOPE:             /* radians/s    */
 509                         num_fields = 3;
 510                         break;
 511
 512                 case SENSOR_TYPE_LIGHT:                 /* SI lux units */
 513                 case SENSOR_TYPE_AMBIENT_TEMPERATURE:   /* °C          */
 514                 case SENSOR_TYPE_TEMPERATURE:           /* °C          */
 515                 case SENSOR_TYPE_PROXIMITY:             /* centimeters  */
 516                 case SENSOR_TYPE_PRESSURE:              /* hecto-pascal */
 517                 case SENSOR_TYPE_RELATIVE_HUMIDITY:     /* percent */
 518                         num_fields = 1;
 519                         break;
 520
 521                 case SENSOR_TYPE_ROTATION_VECTOR:
 522                         num_fields = 4;
 523                         break;
 524
 525                 case SENSOR_TYPE_DEVICE_PRIVATE_BASE:   /* hidden for now */
 526                         num_fields = 0;
 527                         break;
 528
 529                 default:
 530                         ALOGE("Unknown sensor type!\n");
 531                         num_fields = 0;
 532                         break;
 533         }
 534
 535         ALOGV("Sample on sensor %d (type %d):\n", s, sensor_type);
 536
 537         /* If we're dealing with a poll-mode sensor */
 538         if (!sensor_info[s].num_channels) {
 539
 540                 /* Read values through sysfs rather than from a report buffer */
 541                 for (c=0; c<num_fields; c++) {
 542                         val = acquire_immediate_value(s, c);
 543
 544                         data->data[c] = transform_sample(sensor_type, c, val);
 545
 546                         ALOGV("\tfield %d: %f\n", c, data->data[c]);
 547                 }
 548                 return;
 549         }
 550
 551         /* Convert the data into the expected Android-level format */
 552
 553         current_sample = sensor_info[s].report_buffer;
 554
 555         for (c=0; c<num_fields; c++) {
 556                 sample_size     =  sensor_info[s].channel[c].size;
 557                 sample_type     = &sensor_info[s].channel[c].type_info;
 558
 559                 s64 = sample_as_int64(current_sample, sample_type);
 560
 561                 val = (sensor_info[s].offset + s64) * sensor_info[s].scale;
 562
 563                 data->data[c] = transform_sample(sensor_type, c, val);
 564
 565                 ALOGV("\tfield %d: %f\n", c, data->data[c]);
 566                 current_sample += sample_size;
 567         }
 568 }
 569
 570
 571 static int get_poll_time (void)
 572 {
 573         if (!active_poll_sensors)
 574                 return -1;      /* Infinite wait */
 575
 576         return 100;     /* ms ... this needs to be dynamic */
 577 }
 578
 579
 580 int sensor_poll(struct sensors_event_t* data, int count)
 581 {
 582         int s;
 583         int i;
 584         int nfds;
 585         int delta;
 586         struct epoll_event ev[MAX_DEVICES];
 587
 588         /* Get one or more events from our collection of sensors */
 589
 590 return_first_available_sensor_report:
 591
 592         /* If there's at least one available report */
 593         for (s=0; s<sensor_count; s++)
 594                 if (sensor_info[s].report_pending) {
 595
 596                         /* Return that up */
 597                         propagate_sensor_report(s, data);
 598                         sensor_info[s].report_pending = 0;
 599                         ALOGV("Report on sensor %d\n", s);
 600                         return 1;
 601                 }
 602
 603         /* Keep a minimum time interval between poll operations */
 604         delta = (get_timestamp() - last_poll_exit_ts)/1000;
 605
 606         if (delta > 0 && delta < POLL_MIN_INTERVAL)
 607                 usleep(POLL_MIN_INTERVAL - delta);
 608
 609         ALOGV("Awaiting sensor data\n");
 610
 611         nfds = epoll_wait(poll_fd, ev, MAX_DEVICES, get_poll_time());
 612
 613         last_poll_exit_ts = get_timestamp();
 614
 615         ALOGV("%d fds signalled\n", nfds);
 616
 617         /* For each of the devices for which a report is available */
 618         for (i=0; i<nfds; i++)
 619                 if (ev[i].events == EPOLLIN)
 620                         /* Read report */
 621                         integrate_device_report(ev[i].data.u32);
 622
 623         /* It's a good time to invalidate poll-mode sensor values */
 624         for (s=0; s<sensor_count; s++)
 625                 if (!sensor_info[s].num_channels && sensor_info[s].enable_count)
 626                         sensor_info[s].report_pending = 1;
 627
 628         goto return_first_available_sensor_report;
 629 }
 630
 631
 632 int sensor_set_delay(int handle, int64_t ns)
 633 {
 634         /* Set the rate at which a specific sensor should report events */
 635         /* Continuous reports: accelerometer, gyroscope */
 636         /* On change with minimum delay between events: ALS, proximity */
 637         /* See sensors.h for indication on sensor trigger modes */
 638         return -1;
 639 }
 640
 641
 642 int allocate_control_data (void)
 643 {
 644         int i;
 645
 646         for (i=0; i<MAX_DEVICES; i++)
 647                 device_fd[i] = -1;
 648
 649         poll_fd = epoll_create(MAX_DEVICES);
 650
 651         if (poll_fd == -1) {
 652                 ALOGE("Can't create epoll instance for iio sensors!\n");
 653         }
 654
 655         return poll_fd;
 656 }
 657
 658
 659 void delete_control_data (void)
 660 {
 661 }