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