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