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         if (!enabled) {
 329                 if (dev_fd != -1 && !poll_sensors_per_dev[dev_num] &&
 330                         !trig_sensors_per_dev[dev_num]) {
 331                                 /*
 332                                  * Stop watching this fd. This should be a no-op
 333                                  * in case this fd was not in the poll set.
 334                                  */
 335                                 epoll_ctl(poll_fd, EPOLL_CTL_DEL, dev_fd, NULL);
 336
 337                                 close(dev_fd);
 338                                 device_fd[dev_num] = -1;
 339                         }
 340                 return 0;
 341         }
 342
 343         if (dev_fd == -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
 359                 if (!is_poll_sensor) {
 360
 361                         /* Add this iio device fd to the set of watched fds */
 362                         ev.events = EPOLLIN;
 363                         ev.data.u32 = dev_num;
 364
 365                         ret = epoll_ctl(poll_fd, EPOLL_CTL_ADD, dev_fd, &ev);
 366
 367                         if (ret == -1) {
 368                                 ALOGE(  "Failed adding %d to poll set (%s)\n",
 369                                         dev_fd, strerror(errno));
 370                                 return -1;
 371                         }
 372
 373                         /* Note: poll-mode fds are not readable */
 374                 }
 375         }
 376
 377         return 0;
 378 }
 379
 380
 381 static int integrate_device_report(int dev_num)
 382 {
 383         int len;
 384         int s,c;
 385         unsigned char buf[MAX_SENSOR_REPORT_SIZE * MAX_SENSORS] = { 0 };
 386         int sr_offset;
 387         unsigned char *target;
 388         unsigned char *source;
 389         int size;
 390         int expected_size = 0;
 391
 392         /* There's an incoming report on the specified fd */
 393
 394         if (dev_num < 0 || dev_num >= MAX_DEVICES ||
 395                 !trig_sensors_per_dev[dev_num]) {
 396                 ALOGE("Event reported on unexpected iio device %d\n", dev_num);
 397                 return -1;
 398         }
 399
 400         for (s=0; s<MAX_SENSORS; s++)
 401                 if (sensor_info[s].dev_num == dev_num)
 402                         for (c=0; c<sensor_info[s].num_channels; c++)
 403                                 expected_size += sensor_info[s].channel[c].size;
 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         for (s=0; s<MAX_SENSORS; s++)
 416                 if (sensor_info[s].dev_num == dev_num) {
 417                         sr_offset = 0;
 418
 419                         /* Copy data from device to sensor report buffer */
 420                         for (c=0; c<sensor_info[s].num_channels; c++) {
 421
 422                                 target = sensor_info[s].report_buffer +
 423                                         sr_offset;
 424
 425                                 source = buf + sensor_info[s].channel[c].offset;
 426
 427                                 size = sensor_info[s].channel[c].size;
 428
 429                                 memcpy(target, source, size);
 430
 431                                 sr_offset += size;
 432                         }
 433
 434                         if (sensor_info[s].enable_count) {
 435                                 ALOGV("Sensor %d report available (%d bytes)\n",
 436                                       s, sr_offset);
 437
 438                                 sensor_info[s].report_pending = 1;
 439                         }
 440                 }
 441
 442         return 0;
 443 }
 444
 445
 446 static float acquire_immediate_value(int s, int c)
 447 {
 448         char sysfs_path[PATH_MAX];
 449         float val;
 450         int ret;
 451         int dev_num = sensor_info[s].dev_num;
 452         int i = sensor_info[s].catalog_index;
 453         const char* raw_path = sensor_catalog[i].channel[c].raw_path;
 454         const char* input_path = sensor_catalog[i].channel[c].input_path;
 455         float scale = sensor_info[s].scale;
 456         float offset = sensor_info[s].offset;
 457
 458         /* Acquire a sample value for sensor s / channel c through sysfs */
 459
 460         if (input_path[0]) {
 461                 sprintf(sysfs_path, BASE_PATH "%s", dev_num, input_path);
 462                 ret = sysfs_read_float(sysfs_path, &val);
 463
 464                 if (!ret) {
 465                         return val;
 466                 }
 467         };
 468
 469         if (!raw_path[0])
 470                 return 0;
 471
 472         sprintf(sysfs_path, BASE_PATH "%s", dev_num, raw_path);
 473         ret = sysfs_read_float(sysfs_path, &val);
 474
 475         if (ret == -1)
 476                 return 0;
 477
 478         return (val + offset) * scale;
 479 }
 480
 481
 482 static void propagate_sensor_report(int s, struct sensors_event_t* data)
 483 {
 484         /* There's a sensor report pending for this sensor ; transmit it */
 485
 486         int catalog_index = sensor_info[s].catalog_index;
 487         int sensor_type = sensor_catalog[catalog_index].type;
 488         int num_fields;
 489         int c;
 490         unsigned char* current_sample;
 491         int sample_size;
 492         struct datum_info_t* sample_type;
 493         int64_t s64;
 494         float val;
 495
 496         memset(data, 0, sizeof(sensors_event_t));
 497
 498         data->version = sizeof(sensors_event_t);
 499         data->sensor = s;
 500         data->type = sensor_type;
 501         data->timestamp = get_timestamp();
 502
 503         switch (sensor_type) {
 504                 case SENSOR_TYPE_ACCELEROMETER:         /* m/s^2        */
 505                 case SENSOR_TYPE_MAGNETIC_FIELD:        /* micro-tesla  */
 506                 case SENSOR_TYPE_ORIENTATION:           /* degrees      */
 507                 case SENSOR_TYPE_GYROSCOPE:             /* radians/s    */
 508                         num_fields = 3;
 509                         break;
 510
 511                 case SENSOR_TYPE_LIGHT:                 /* SI lux units */
 512                 case SENSOR_TYPE_AMBIENT_TEMPERATURE:   /* °C          */
 513                 case SENSOR_TYPE_TEMPERATURE:           /* °C          */
 514                 case SENSOR_TYPE_PROXIMITY:             /* centimeters  */
 515                 case SENSOR_TYPE_PRESSURE:              /* hecto-pascal */
 516                 case SENSOR_TYPE_RELATIVE_HUMIDITY:     /* percent */
 517                         num_fields = 1;
 518                         break;
 519
 520                 case SENSOR_TYPE_ROTATION_VECTOR:
 521                         num_fields = 4;
 522                         break;
 523
 524                 case SENSOR_TYPE_DEVICE_PRIVATE_BASE:   /* hidden for now */
 525                         num_fields = 0;
 526                         break;
 527
 528                 default:
 529                         ALOGE("Unknown sensor type!\n");
 530                         num_fields = 0;
 531                         break;
 532         }
 533
 534         ALOGV("Sample on sensor %d (type %d):\n", s, sensor_type);
 535
 536         /* If we're dealing with a poll-mode sensor */
 537         if (!sensor_info[s].num_channels) {
 538
 539                 /* Read values through sysfs rather than from a report buffer */
 540                 for (c=0; c<num_fields; c++) {
 541                         val = acquire_immediate_value(s, c);
 542
 543                         data->data[c] = transform_sample(sensor_type, c, val);
 544
 545                         ALOGV("\tfield %d: %f\n", c, data->data[c]);
 546                 }
 547                 return;
 548         }
 549
 550         /* Convert the data into the expected Android-level format */
 551
 552         current_sample = sensor_info[s].report_buffer;
 553
 554         for (c=0; c<num_fields; c++) {
 555                 sample_size     =  sensor_info[s].channel[c].size;
 556                 sample_type     = &sensor_info[s].channel[c].type_info;
 557
 558                 s64 = sample_as_int64(current_sample, sample_type);
 559
 560                 val = (sensor_info[s].offset + s64) * sensor_info[s].scale;
 561
 562                 data->data[c] = transform_sample(sensor_type, c, val);
 563
 564                 ALOGV("\tfield %d: %f\n", c, data->data[c]);
 565                 current_sample += sample_size;
 566         }
 567 }
 568
 569
 570 static int get_poll_time (void)
 571 {
 572         if (!active_poll_sensors)
 573                 return -1;      /* Infinite wait */
 574
 575         return 100;     /* ms ... this needs to be dynamic */
 576 }
 577
 578
 579 int sensor_poll(struct sensors_event_t* data, int count)
 580 {
 581         int s;
 582         int i;
 583         int nfds;
 584         int delta;
 585         struct epoll_event ev[MAX_DEVICES];
 586
 587         /* Get one or more events from our collection of sensors */
 588
 589 return_first_available_sensor_report:
 590
 591         /* If there's at least one available report */
 592         for (s=0; s<sensor_count; s++)
 593                 if (sensor_info[s].report_pending) {
 594
 595                         /* Return that up */
 596                         propagate_sensor_report(s, data);
 597                         sensor_info[s].report_pending = 0;
 598                         ALOGV("Report on sensor %d\n", s);
 599                         return 1;
 600                 }
 601
 602         /* Keep a minimum time interval between poll operations */
 603         delta = (get_timestamp() - last_poll_exit_ts)/1000;
 604
 605         if (delta > 0 && delta < POLL_MIN_INTERVAL)
 606                 usleep(POLL_MIN_INTERVAL - delta);
 607
 608         ALOGV("Awaiting sensor data\n");
 609
 610         nfds = epoll_wait(poll_fd, ev, MAX_DEVICES, get_poll_time());
 611
 612         last_poll_exit_ts = get_timestamp();
 613
 614         ALOGV("%d fds signalled\n", nfds);
 615
 616         /* For each of the devices for which a report is available */
 617         for (i=0; i<nfds; i++)
 618                 if (ev[i].events == EPOLLIN)
 619                         /* Read report */
 620                         integrate_device_report(ev[i].data.u32);
 621
 622         /* It's a good time to invalidate poll-mode sensor values */
 623         if (active_poll_sensors)
 624                 for (s=0; s<sensor_count; s++)
 625                         if (sensor_info[s].enable_count &&
 626                                 !sensor_info[s].num_channels)
 627                                         sensor_info[s].report_pending = 1;
 628
 629         goto return_first_available_sensor_report;
 630 }
 631
 632
 633 int sensor_set_delay(int handle, int64_t ns)
 634 {
 635         /* Set the rate at which a specific sensor should report events */
 636         /* Continuous reports: accelerometer, gyroscope */
 637         /* On change with minimum delay between events: ALS, proximity */
 638         /* See sensors.h for indication on sensor trigger modes */
 639         return -1;
 640 }
 641
 642
 643 int allocate_control_data (void)
 644 {
 645         int i;
 646
 647         for (i=0; i<MAX_DEVICES; i++)
 648                 device_fd[i] = -1;
 649
 650         poll_fd = epoll_create(MAX_DEVICES);
 651
 652         if (poll_fd == -1) {
 653                 ALOGE("Can't create epoll instance for iio sensors!\n");
 654         }
 655
 656         return poll_fd;
 657 }
 658
 659
 660 void delete_control_data (void)
 661 {
 662 }