2 * Copyright (C) 2014 Intel Corporation.
9 #include <sys/socket.h>
10 #include <utils/Log.h>
11 #include <hardware/sensors.h>
13 #include "enumeration.h"
15 #include "transform.h"
16 #include "calibration.h"
18 /* Currently active sensors count, per device */
19 static int poll_sensors_per_dev[MAX_DEVICES]; /* poll-mode sensors */
20 static int trig_sensors_per_dev[MAX_DEVICES]; /* trigger, event based */
22 static int device_fd[MAX_DEVICES]; /* fd on the /dev/iio:deviceX file */
24 static int poll_fd; /* epoll instance covering all enabled sensors */
26 static int poll_socket_pair[2]; /* used to unblock the poll loop */
28 /* Timestamp for the moment when we last exited a poll operation */
29 static int64_t last_poll_exit_ts;
31 static int active_poll_sensors; /* Number of enabled poll-mode sensors */
33 #define INVALID_DEV_NUM ((uint32_t) -1)
36 static int enable_buffer(int dev_num, int enabled)
38 char sysfs_path[PATH_MAX];
40 sprintf(sysfs_path, ENABLE_PATH, dev_num);
42 /* Low level, non-multiplexed, enable/disable routine */
43 return sysfs_write_int(sysfs_path, enabled);
47 static int setup_trigger(int dev_num, const char* trigger_val)
49 char sysfs_path[PATH_MAX];
51 sprintf(sysfs_path, TRIGGER_PATH, dev_num);
53 return sysfs_write_str(sysfs_path, trigger_val);
57 void build_sensor_report_maps(int dev_num)
60 * Read sysfs files from a iio device's scan_element directory, and
61 * build a couple of tables from that data. These tables will tell, for
62 * each sensor, where to gather relevant data in a device report, i.e.
63 * the structure that we read from the /dev/iio:deviceX file in order to
64 * sensor report, itself being the data that we return to Android when a
65 * sensor poll completes. The mapping should be straightforward in the
66 * case where we have a single sensor active per iio device but, this is
67 * not the general case. In general several sensors can be handled
68 * through a single iio device, and the _en, _index and _type syfs
69 * entries all concur to paint a picture of what the structure of the
79 char spec_buf[MAX_TYPE_SPEC_LEN];
80 struct datum_info_t* ch_info;
82 char sysfs_path[PATH_MAX];
85 int channel_size_from_index[MAX_SENSORS * MAX_CHANNELS] = { 0 };
86 int sensor_handle_from_index[MAX_SENSORS * MAX_CHANNELS] = { 0 };
87 int channel_number_from_index[MAX_SENSORS * MAX_CHANNELS] = { 0 };
91 /* For each sensor that is linked to this device */
92 for (s=0; s<sensor_count; s++) {
93 if (sensor_info[s].dev_num != dev_num)
96 i = sensor_info[s].catalog_index;
98 /* Read channel details through sysfs attributes */
99 for (c=0; c<sensor_info[s].num_channels; c++) {
101 /* Read _type file */
102 sprintf(sysfs_path, CHANNEL_PATH "%s",
103 sensor_info[s].dev_num,
104 sensor_catalog[i].channel[c].type_path);
106 n = sysfs_read_str(sysfs_path, spec_buf,
110 ALOGW( "Failed to read type: %s\n",
115 ch_spec = sensor_info[s].channel[c].type_spec;
117 memcpy(ch_spec, spec_buf, sizeof(spec_buf));
119 ch_info = &sensor_info[s].channel[c].type_info;
121 size = decode_type_spec(ch_spec, ch_info);
123 /* Read _index file */
124 sprintf(sysfs_path, CHANNEL_PATH "%s",
125 sensor_info[s].dev_num,
126 sensor_catalog[i].channel[c].index_path);
128 n = sysfs_read_int(sysfs_path, &ch_index);
131 ALOGW( "Failed to read index: %s\n",
136 if (ch_index >= MAX_SENSORS) {
137 ALOGE("Index out of bounds!: %s\n", sysfs_path);
141 /* Record what this index is about */
143 sensor_handle_from_index [ch_index] = s;
144 channel_number_from_index[ch_index] = c;
145 channel_size_from_index [ch_index] = size;
150 /* Stop sampling - if we are recovering from hal restart */
151 enable_buffer(dev_num, 0);
152 setup_trigger(dev_num, "\n");
154 /* Turn on channels we're aware of */
155 for (c=0;c<sensor_info[s].num_channels; c++) {
156 sprintf(sysfs_path, CHANNEL_PATH "%s",
157 sensor_info[s].dev_num,
158 sensor_catalog[i].channel[c].en_path);
159 sysfs_write_int(sysfs_path, 1);
163 ALOGI("Found %d channels on iio device %d\n", known_channels, dev_num);
166 * Now that we know which channels are defined, their sizes and their
167 * ordering, update channels offsets within device report. Note: there
168 * is a possibility that several sensors share the same index, with
169 * their data fields being isolated by masking and shifting as specified
170 * through the real bits and shift values in type attributes. This case
171 * is not currently supported. Also, the code below assumes no hole in
172 * the sequence of indices, so it is dependent on discovery of all
176 for (i=0; i<MAX_SENSORS * MAX_CHANNELS; i++) {
177 s = sensor_handle_from_index[i];
178 c = channel_number_from_index[i];
179 size = channel_size_from_index[i];
184 ALOGI("S%d C%d : offset %d, size %d, type %s\n",
185 s, c, offset, size, sensor_info[s].channel[c].type_spec);
187 sensor_info[s].channel[c].offset = offset;
188 sensor_info[s].channel[c].size = size;
195 int adjust_counters (int s, int enabled)
198 * Adjust counters based on sensor enable action. Return values are:
199 * -1 if there's an inconsistency: abort action in this case
200 * 0 if the operation was completed and we're all set
201 * 1 if we toggled the state of the sensor and there's work left
204 int dev_num = sensor_info[s].dev_num;
205 int catalog_index = sensor_info[s].catalog_index;
206 int sensor_type = sensor_catalog[catalog_index].type;
208 /* Refcount per sensor, in terms of enable count */
210 ALOGI("Enabling sensor %d (iio device %d: %s)\n",
211 s, dev_num, sensor_info[s].friendly_name);
213 sensor_info[s].enable_count++;
215 if (sensor_info[s].enable_count != 1) {
216 return 0; /* The sensor was, and remains, in use */
218 if (sensor_type == SENSOR_TYPE_MAGNETIC_FIELD)
219 compass_read_data(&sensor_info[s]);
221 if (sensor_type == SENSOR_TYPE_GYROSCOPE)
222 gyro_cal_init(&sensor_info[s]);
225 if (sensor_info[s].enable_count == 0)
226 return -1; /* Spurious disable call */
228 ALOGI("Disabling sensor %d (iio device %d: %s)\n", s, dev_num,
229 sensor_info[s].friendly_name);
231 if (sensor_type == SENSOR_TYPE_MAGNETIC_FIELD)
232 compass_store_data(&sensor_info[s]);
234 sensor_info[s].enable_count--;
236 if (sensor_info[s].enable_count > 0)
237 return 0; /* The sensor was, and remains, in use */
239 /* Sensor disabled, lower report available flag */
240 sensor_info[s].report_pending = 0;
243 /* We changed the state of a sensor - adjust per iio device counters */
245 /* If this is a regular event-driven sensor */
246 if (sensor_info[s].num_channels) {
249 trig_sensors_per_dev[dev_num]++;
251 trig_sensors_per_dev[dev_num]--;
257 active_poll_sensors++;
258 poll_sensors_per_dev[dev_num]++;
262 active_poll_sensors--;
263 poll_sensors_per_dev[dev_num]--;
268 int sensor_activate(int s, int enabled)
270 char device_name[PATH_MAX];
271 char trigger_name[MAX_NAME_SIZE + 16];
273 struct epoll_event ev = {0};
276 int dev_num = sensor_info[s].dev_num;
277 int i = sensor_info[s].catalog_index;
278 int is_poll_sensor = !sensor_info[s].num_channels;
280 ret = adjust_counters(s, enabled);
282 /* If the operation was neutral in terms of state, we're done */
286 if (!is_poll_sensor) {
289 enable_buffer(dev_num, 0);
290 setup_trigger(dev_num, "\n");
292 /* If there's at least one sensor enabled on this iio device */
293 if (trig_sensors_per_dev[dev_num]) {
294 sprintf(trigger_name, "%s-dev%d",
295 sensor_info[s].internal_name, dev_num);
298 setup_trigger(dev_num, trigger_name);
299 enable_buffer(dev_num, 1);
304 * Make sure we have a fd on the character device ; conversely, close
305 * the fd if no one is using associated sensors anymore. The assumption
306 * here is that the underlying driver will power on the relevant
307 * hardware block while someone holds a fd on the device.
309 dev_fd = device_fd[dev_num];
312 if (dev_fd != -1 && !poll_sensors_per_dev[dev_num] &&
313 !trig_sensors_per_dev[dev_num]) {
315 * Stop watching this fd. This should be a no-op
316 * in case this fd was not in the poll set.
318 epoll_ctl(poll_fd, EPOLL_CTL_DEL, dev_fd, NULL);
321 device_fd[dev_num] = -1;
327 /* First enabled sensor on this iio device */
328 sprintf(device_name, DEV_FILE_PATH, dev_num);
329 dev_fd = open(device_name, O_RDONLY | O_NONBLOCK);
331 device_fd[dev_num] = dev_fd;
334 ALOGE("Could not open fd on %s (%s)\n",
335 device_name, strerror(errno));
336 adjust_counters(s, 0);
340 ALOGV("Opened %s: fd=%d\n", device_name, dev_fd);
342 if (!is_poll_sensor) {
344 /* Add this iio device fd to the set of watched fds */
346 ev.data.u32 = dev_num;
348 ret = epoll_ctl(poll_fd, EPOLL_CTL_ADD, dev_fd, &ev);
351 ALOGE( "Failed adding %d to poll set (%s)\n",
352 dev_fd, strerror(errno));
356 /* Note: poll-mode fds are not readable */
360 /* Release the polling loop so an updated timeout gets used */
361 write(poll_socket_pair[1], "", 1);
367 static int integrate_device_report(int dev_num)
371 unsigned char buf[MAX_SENSOR_REPORT_SIZE] = { 0 };
373 unsigned char *target;
374 unsigned char *source;
378 /* There's an incoming report on the specified fd */
380 if (dev_num < 0 || dev_num >= MAX_DEVICES) {
381 ALOGE("Event reported on unexpected iio device %d\n", dev_num);
385 if (device_fd[dev_num] == -1) {
386 ALOGE("Ignoring stale report on iio device %d\n", dev_num);
390 ts = get_timestamp();
392 len = read(device_fd[dev_num], buf, MAX_SENSOR_REPORT_SIZE);
395 ALOGE("Could not read report from iio device %d (%s)\n",
396 dev_num, strerror(errno));
400 ALOGV("Read %d bytes from iio device %d\n", len, dev_num);
402 for (s=0; s<MAX_SENSORS; s++)
403 if (sensor_info[s].dev_num == dev_num &&
404 sensor_info[s].enable_count) {
408 /* Copy data from device to sensor report buffer */
409 for (c=0; c<sensor_info[s].num_channels; c++) {
411 target = sensor_info[s].report_buffer +
414 source = buf + sensor_info[s].channel[c].offset;
416 size = sensor_info[s].channel[c].size;
418 memcpy(target, source, size);
423 ALOGV("Sensor %d report available (%d bytes)\n", s,
426 sensor_info[s].report_ts = ts;
427 sensor_info[s].report_pending = 1;
434 static int propagate_sensor_report(int s, struct sensors_event_t* data)
436 /* There's a sensor report pending for this sensor ; transmit it */
438 int catalog_index = sensor_info[s].catalog_index;
439 int sensor_type = sensor_catalog[catalog_index].type;
442 unsigned char* current_sample;
443 int64_t current_ts = get_timestamp();
446 memset(data, 0, sizeof(sensors_event_t));
448 data->version = sizeof(sensors_event_t);
450 data->type = sensor_type;
452 if (sensor_info[s].report_ts)
453 data->timestamp = sensor_info[s].report_ts;
455 data->timestamp = current_ts;
457 switch (sensor_type) {
458 case SENSOR_TYPE_ACCELEROMETER: /* m/s^2 */
459 case SENSOR_TYPE_MAGNETIC_FIELD: /* micro-tesla */
460 case SENSOR_TYPE_ORIENTATION: /* degrees */
461 case SENSOR_TYPE_GYROSCOPE: /* radians/s */
465 case SENSOR_TYPE_LIGHT: /* SI lux units */
466 case SENSOR_TYPE_AMBIENT_TEMPERATURE: /* °C */
467 case SENSOR_TYPE_TEMPERATURE: /* °C */
468 case SENSOR_TYPE_PROXIMITY: /* centimeters */
469 case SENSOR_TYPE_PRESSURE: /* hecto-pascal */
470 case SENSOR_TYPE_RELATIVE_HUMIDITY: /* percent */
474 case SENSOR_TYPE_ROTATION_VECTOR:
478 case SENSOR_TYPE_DEVICE_PRIVATE_BASE: /* Hidden for now */
479 return 0; /* Drop sample */
482 ALOGE("Unknown sensor type!\n");
483 return 0; /* Drop sample */
486 ALOGV("Sample on sensor %d (type %d):\n", s, sensor_type);
488 /* Take note of current time counter value for rate control purposes */
489 sensor_info[s].last_integration_ts = current_ts;
491 /* If we're dealing with a poll-mode sensor */
492 if (!sensor_info[s].num_channels) {
494 /* Read values through sysfs rather than from a report buffer */
495 for (c=0; c<num_fields; c++) {
497 data->data[c] = acquire_immediate_value(s, c);
499 ALOGV("\tfield %d: %f\n", c, data->data[c]);
502 /* Control acquisition time and flag anything beyond 100 ms */
503 delta = get_timestamp() - current_ts;
505 if (delta > 100 * 1000 * 1000) {
506 ALOGI("Sensor %d (%s) sampling time: %d ms\n", s,
507 sensor_info[s].friendly_name, (int) (delta / 1000000));
510 return sensor_info[s].ops.finalize(s, data);
513 /* Convert the data into the expected Android-level format */
515 current_sample = sensor_info[s].report_buffer;
517 for (c=0; c<num_fields; c++) {
519 data->data[c] = sensor_info[s].ops.transform
520 (s, c, current_sample);
522 ALOGV("\tfield %d: %f\n", c, data->data[c]);
523 current_sample += sensor_info[s].channel[c].size;
527 * The finalize routine, in addition to its late sample processing duty,
528 * has the final say on whether or not the sample gets sent to Android.
530 return sensor_info[s].ops.finalize(s, data);
534 static int get_poll_time (void)
537 int64_t lowest_target_ts;
541 if (!active_poll_sensors)
542 return -1; /* Infinite wait */
544 /* Check if we should schedule a poll-mode sensor event delivery */
546 lowest_target_ts = INT64_MAX;
548 for (s=0; s<sensor_count; s++)
549 if (sensor_info[s].enable_count &&
550 sensor_info[s].sampling_rate > 0 &&
551 !sensor_info[s].num_channels) {
552 target_ts = sensor_info[s].last_integration_ts +
553 1000000000LL/sensor_info[s].sampling_rate;
555 if (target_ts < lowest_target_ts)
556 lowest_target_ts = target_ts;
559 if (lowest_target_ts == INT64_MAX)
562 current_ts = get_timestamp();
564 if (lowest_target_ts <= current_ts)
567 return (lowest_target_ts - current_ts)/1000000; /* ms */
571 static void acknowledge_release (void)
573 /* A write to our socket circuit was performed to release epoll */
575 read(poll_socket_pair[0], &buf, 1);
579 int sensor_poll(struct sensors_event_t* data, int count)
585 struct epoll_event ev[MAX_DEVICES];
588 /* Get one or more events from our collection of sensors */
590 return_first_available_sensor_report:
592 /* If there's at least one available report */
593 for (s=0; s<sensor_count; s++)
594 if (sensor_info[s].report_pending) {
597 sensor_info[s].report_pending = 0;
599 if (propagate_sensor_report(s, data)) {
601 ALOGV("Report on sensor %d\n", s);
606 * If the sample was deemed invalid or unreportable,
607 * e.g. had the same value as the previously reported
608 * value for a 'on change' sensor, silently drop it
613 ALOGV("Awaiting sensor data\n");
615 nfds = epoll_wait(poll_fd, ev, MAX_DEVICES, get_poll_time());
617 last_poll_exit_ts = get_timestamp();
620 ALOGI("epoll_wait returned -1 (%s)\n", strerror(errno));
624 ALOGV("%d fds signalled\n", nfds);
626 /* For each of the devices for which a report is available */
627 for (i=0; i<nfds; i++)
628 if (ev[i].events == EPOLLIN) {
629 if (ev[i].data.u32 == INVALID_DEV_NUM) {
630 acknowledge_release();
634 integrate_device_report(ev[i].data.u32);
637 /* Check poll-mode sensors and fire up an event if it's time to do so */
638 if (active_poll_sensors)
639 for (s=0; s<sensor_count; s++)
640 if (sensor_info[s].enable_count &&
641 !sensor_info[s].num_channels &&
642 sensor_info[s].sampling_rate > 0) {
643 target_ts = sensor_info[s].last_integration_ts +
644 1000000000LL/sensor_info[s].sampling_rate;
646 if (last_poll_exit_ts >= target_ts)
647 sensor_info[s].report_pending = 1;
650 goto return_first_available_sensor_report;
654 int sensor_set_delay(int s, int64_t ns)
656 /* Set the rate at which a specific sensor should report events */
658 /* See Android sensors.h for indication on sensor trigger modes */
660 char sysfs_path[PATH_MAX];
661 char avail_sysfs_path[PATH_MAX];
662 int dev_num = sensor_info[s].dev_num;
663 int i = sensor_info[s].catalog_index;
664 const char *prefix = sensor_catalog[i].tag;
665 float new_sampling_rate; /* Granted sampling rate after arbitration */
666 float cur_sampling_rate; /* Currently used sampling rate */
667 float req_sampling_rate; /* Requested ; may be different from granted */
668 int per_sensor_sampling_rate;
669 int per_device_sampling_rate;
670 int max_supported_rate = 0;
677 ALOGE("Rejecting zero delay request on sensor %d\n", s);
681 new_sampling_rate = req_sampling_rate = 1000000000L/ns;
683 if (new_sampling_rate < 1) {
684 ALOGI("Sub-HZ sampling rate requested on on sensor %d\n", s);
685 new_sampling_rate = 1;
688 sensor_info[s].sampling_rate = new_sampling_rate;
690 /* If we're dealing with a poll-mode sensor, release poll and return */
691 if (!sensor_info[s].num_channels)
694 sprintf(sysfs_path, SENSOR_SAMPLING_PATH, dev_num, prefix);
696 if (sysfs_read_float(sysfs_path, &cur_sampling_rate) != -1) {
697 per_sensor_sampling_rate = 1;
698 per_device_sampling_rate = 0;
700 per_sensor_sampling_rate = 0;
702 sprintf(sysfs_path, DEVICE_SAMPLING_PATH, dev_num);
704 if (sysfs_read_float(sysfs_path, &cur_sampling_rate) != -1)
705 per_device_sampling_rate = 1;
707 per_device_sampling_rate = 0;
710 if (!per_sensor_sampling_rate && !per_device_sampling_rate) {
711 ALOGE("No way to adjust sampling rate on sensor %d\n", s);
715 /* Coordinate with others active sensors on the same device, if any */
716 if (per_device_sampling_rate)
717 for (n=0; n<sensor_count; n++)
718 if (n != s && sensor_info[n].dev_num == dev_num &&
719 sensor_info[n].num_channels &&
720 sensor_info[n].enable_count &&
721 sensor_info[n].sampling_rate > new_sampling_rate)
722 new_sampling_rate= sensor_info[n].sampling_rate;
724 /* Check if we have contraints on allowed sampling rates */
726 sprintf(avail_sysfs_path, DEVICE_AVAIL_FREQ_PATH, dev_num);
728 if (sysfs_read_str(avail_sysfs_path, freqs_buf, sizeof(freqs_buf)) > 0){
731 /* Decode allowed sampling rates string, ex: "10 20 50 100" */
733 /* While we're not at the end of the string */
734 while (*cursor && cursor[0]) {
736 /* Decode a single value */
737 sr = strtod(cursor, NULL);
739 /* Cap sampling rate to CAP_SENSOR_MAX_FREQUENCY*/
740 if (sr > CAP_SENSOR_MAX_FREQUENCY)
743 if (sr > max_supported_rate)
744 max_supported_rate = sr;
746 /* If this matches the selected rate, we're happy */
747 if (new_sampling_rate == sr)
751 * If we reached a higher value than the desired rate,
752 * adjust selected rate so it matches the first higher
753 * available one and stop parsing - this makes the
754 * assumption that rates are sorted by increasing value
755 * in the allowed frequencies string.
757 if (sr > new_sampling_rate) {
759 "Increasing sampling rate on sensor %d from %g to %g\n",
760 s, (double) req_sampling_rate, (double) sr);
762 new_sampling_rate = sr;
767 while (cursor[0] && !isspace(cursor[0]))
771 while (cursor[0] && isspace(cursor[0]))
777 if (max_supported_rate &&
778 new_sampling_rate > max_supported_rate) {
779 new_sampling_rate = max_supported_rate;
780 ALOGI( "Can't support %g sampling rate, lowering to %g\n",
781 (double) req_sampling_rate, (double) new_sampling_rate);
785 /* If the desired rate is already active we're all set */
786 if (new_sampling_rate == cur_sampling_rate)
789 ALOGI("Sensor %d sampling rate switched to %g\n", s, new_sampling_rate);
791 if (trig_sensors_per_dev[dev_num])
792 enable_buffer(dev_num, 0);
794 sysfs_write_float(sysfs_path, new_sampling_rate);
796 if (trig_sensors_per_dev[dev_num])
797 enable_buffer(dev_num, 1);
800 /* Release the polling loop so an updated timeout value gets used */
801 write(poll_socket_pair[1], "", 1);
808 int allocate_control_data (void)
811 struct epoll_event ev = {0};
813 for (i=0; i<MAX_DEVICES; i++)
816 poll_fd = epoll_create(MAX_DEVICES);
819 ALOGE("Can't create epoll instance for iio sensors!\n");
823 /* Create and add "unblocking" fd to the set of watched fds */
825 if (socketpair(AF_UNIX, SOCK_STREAM, 0, poll_socket_pair) == -1) {
826 ALOGE("Can't create socket pair for iio sensors!\n");
832 ev.data.u32 = INVALID_DEV_NUM;
834 epoll_ctl(poll_fd, EPOLL_CTL_ADD, poll_socket_pair[0], &ev);
840 void delete_control_data (void)