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"
17 /* Currently active sensors count, per device */
18 static int poll_sensors_per_dev[MAX_DEVICES]; /* poll-mode sensors */
19 static int trig_sensors_per_dev[MAX_DEVICES]; /* trigger, event based */
21 static int device_fd[MAX_DEVICES]; /* fd on the /dev/iio:deviceX file */
23 static int poll_fd; /* epoll instance covering all enabled sensors */
25 static int poll_socket_pair[2]; /* used to unblock the poll loop */
27 /* Timestamp for the moment when we last exited a poll operation */
28 static int64_t last_poll_exit_ts;
30 static int active_poll_sensors; /* Number of enabled poll-mode sensors */
32 /* Cap the time between poll operations to this, to counter runaway polls */
33 #define POLL_MIN_INTERVAL 10000 /* uS */
35 #define INVALID_DEV_NUM ((uint32_t) -1)
38 static int enable_buffer(int dev_num, int enabled)
40 char sysfs_path[PATH_MAX];
42 sprintf(sysfs_path, ENABLE_PATH, dev_num);
44 /* Low level, non-multiplexed, enable/disable routine */
45 return sysfs_write_int(sysfs_path, enabled);
49 static int setup_trigger(int dev_num, const char* trigger_val)
51 char sysfs_path[PATH_MAX];
53 sprintf(sysfs_path, TRIGGER_PATH, dev_num);
55 return sysfs_write_str(sysfs_path, trigger_val);
59 static void refresh_sensor_report_maps(int dev_num)
62 * Read sysfs files from a iio device's scan_element directory, and
63 * build a couple of tables from that data. These tables will tell, for
64 * each sensor, where to gather relevant data in a device report, i.e.
65 * the structure that we read from the /dev/iio:deviceX file in order to
66 * sensor report, itself being the data that we return to Android when a
67 * sensor poll completes. The mapping should be straightforward in the
68 * case where we have a single sensor active per iio device but, this is
69 * not the general case. In general several sensors can be handled
70 * through a single iio device, and the _en, _index and _type syfs
71 * entries all concur to paint a picture of what the structure of the
82 char spec_buf[MAX_TYPE_SPEC_LEN];
83 struct datum_info_t* ch_info;
85 char sysfs_path[PATH_MAX];
89 int channel_size_from_index[MAX_SENSORS * MAX_CHANNELS] = { 0 };
90 int sensor_handle_from_index[MAX_SENSORS * MAX_CHANNELS] = { 0 };
91 int channel_number_from_index[MAX_SENSORS * MAX_CHANNELS] = { 0 };
95 /* For each sensor that is linked to this device */
96 for (s=0; s<sensor_count; s++) {
97 if (sensor_info[s].dev_num != dev_num)
100 i = sensor_info[s].catalog_index;
102 /* Read channel status through syfs attributes */
103 for (c=0; c<sensor_info[s].num_channels; c++) {
106 sprintf(sysfs_path, CHANNEL_PATH "%s",
107 sensor_info[s].dev_num,
108 sensor_catalog[i].channel[c].en_path);
110 n = sysfs_read_int(sysfs_path, &ch_enabled);
113 ALOGW( "Failed to read _en flag: %s\n",
118 if (!ch_enabled != 1) {
119 sensor_info[s].channel[c].size = 0;
122 /* Read _type file */
123 sprintf(sysfs_path, CHANNEL_PATH "%s",
124 sensor_info[s].dev_num,
125 sensor_catalog[i].channel[c].type_path);
127 n = sysfs_read_str(sysfs_path, spec_buf,
131 ALOGW( "Failed to read type: %s\n",
136 ch_spec = sensor_info[s].channel[c].type_spec;
138 memcpy(ch_spec, spec_buf, sizeof(spec_buf));
140 ch_info = &sensor_info[s].channel[c].type_info;
142 size = decode_type_spec(ch_spec, ch_info);
144 /* Read _index file */
145 sprintf(sysfs_path, CHANNEL_PATH "%s",
146 sensor_info[s].dev_num,
147 sensor_catalog[i].channel[c].index_path);
149 n = sysfs_read_int(sysfs_path, &ch_index);
152 ALOGW( "Failed to read index: %s\n",
157 if (ch_index >= MAX_SENSORS) {
158 ALOGE("Index out of bounds!: %s\n", sysfs_path);
162 /* Record what this index is about */
164 sensor_handle_from_index [ch_index] = s;
165 channel_number_from_index[ch_index] = c;
166 channel_size_from_index [ch_index] = size;
172 ALOGI("Found %d enabled channels for iio device %d\n", active_channels,
176 * Now that we know which channels are enabled, their sizes and their
177 * ordering, update channels offsets within device report. Note: there
178 * is a possibility that several sensors share the same index, with
179 * their data fields being isolated by masking and shifting as specified
180 * through the real bits and shift values in type attributes. This case
181 * is not currently supported. Also, the code below assumes no hole in
182 * the sequence of indices, so it is dependent on discovery of all
186 for (i=0; i<MAX_SENSORS * MAX_CHANNELS; i++) {
187 s = sensor_handle_from_index[i];
188 c = channel_number_from_index[i];
189 size = channel_size_from_index[i];
194 ALOGI("S%d C%d : offset %d, size %d, type %s\n",
195 s, c, offset, size, sensor_info[s].channel[c].type_spec);
197 sensor_info[s].channel[c].offset = offset;
198 sensor_info[s].channel[c].size = size;
205 int adjust_counters (int s, int enabled)
208 * Adjust counters based on sensor enable action. Return values are:
209 * -1 if there's an inconsistency: abort action in this case
210 * 0 if the operation was completed and we're all set
211 * 1 if we toggled the state of the sensor and there's work left
214 int dev_num = sensor_info[s].dev_num;
216 /* Refcount per sensor, in terms of enable count */
218 ALOGI("Enabling sensor %d (iio device %d: %s)\n",
219 s, dev_num, sensor_info[s].friendly_name);
221 sensor_info[s].enable_count++;
223 if (sensor_info[s].enable_count != 1)
224 return 0; /* The sensor was, and remains, in use */
226 if (sensor_info[s].enable_count == 0)
227 return -1; /* Spurious disable call */
229 ALOGI("Disabling sensor %d (iio device %d: %s)\n", s, dev_num,
230 sensor_info[s].friendly_name);
232 sensor_info[s].enable_count--;
234 if (sensor_info[s].enable_count > 0)
235 return 0; /* The sensor was, and remains, in use */
237 /* Sensor disabled, clear up pending data */
239 sensor_info[s].report_pending = 0;
240 memset(sensor_info[s].report_buffer, 0, MAX_SENSOR_REPORT_SIZE);
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 sysfs_path[PATH_MAX];
271 char device_name[PATH_MAX];
272 char trigger_name[MAX_NAME_SIZE + 16];
274 struct epoll_event ev = {0};
277 int dev_num = sensor_info[s].dev_num;
278 int i = sensor_info[s].catalog_index;
279 int is_poll_sensor = !sensor_info[s].num_channels;
281 ret = adjust_counters(s, enabled);
283 /* If the operation was neutral in terms of state, we're done */
287 if (!is_poll_sensor) {
288 /* Changes have to be made while the buffer is turned off */
289 enable_buffer(dev_num, 0);
291 /* Configure trigger */
292 switch (trig_sensors_per_dev[dev_num]) {
294 setup_trigger(dev_num, "none");
298 sprintf(trigger_name, "%s-dev%d",
299 sensor_info[s].internal_name, dev_num);
301 setup_trigger(dev_num, trigger_name);
305 /* The trigger is already set */
310 * Turn channels associated to this sensor on or off, and update
311 * the channels maps for all sensors associated to this device.
313 for (c=0;c<sensor_info[s].num_channels; c++) {
314 sprintf(sysfs_path, CHANNEL_PATH "%s",
315 sensor_info[s].dev_num,
316 sensor_catalog[i].channel[c].en_path);
318 sysfs_write_int(sysfs_path, enabled);
321 /* If there's at least one sensor left */
322 if (trig_sensors_per_dev[dev_num]) {
323 refresh_sensor_report_maps(dev_num);
324 enable_buffer(dev_num, 1);
329 * Make sure we have a fd on the character device ; conversely, close
330 * the fd if no one is using associated sensor anymore. The assumption
331 * here is that the underlying driver will power on the relevant
332 * hardware block while someone hold a fd on the device.
334 dev_fd = device_fd[dev_num];
337 if (dev_fd != -1 && !poll_sensors_per_dev[dev_num] &&
338 !trig_sensors_per_dev[dev_num]) {
340 * Stop watching this fd. This should be a no-op
341 * in case this fd was not in the poll set.
343 epoll_ctl(poll_fd, EPOLL_CTL_DEL, dev_fd, NULL);
346 device_fd[dev_num] = -1;
352 /* First enabled sensor on this iio device */
353 sprintf(device_name, DEV_FILE_PATH, dev_num);
354 dev_fd = open(device_name, O_RDONLY | O_NONBLOCK);
356 device_fd[dev_num] = dev_fd;
359 ALOGE("Could not open fd on %s (%s)\n",
360 device_name, strerror(errno));
361 adjust_counters(s, 0);
365 ALOGV("Opened %s: fd=%d\n", device_name, dev_fd);
367 if (!is_poll_sensor) {
369 /* Add this iio device fd to the set of watched fds */
371 ev.data.u32 = dev_num;
373 ret = epoll_ctl(poll_fd, EPOLL_CTL_ADD, dev_fd, &ev);
376 ALOGE( "Failed adding %d to poll set (%s)\n",
377 dev_fd, strerror(errno));
381 /* Note: poll-mode fds are not readable */
385 /* Release the polling loop so an updated timeout gets used */
386 write(poll_socket_pair[1], "", 1);
392 static int integrate_device_report(int dev_num)
396 unsigned char buf[MAX_SENSOR_REPORT_SIZE * MAX_SENSORS] = { 0 };
398 unsigned char *target;
399 unsigned char *source;
401 int expected_size = 0;
403 /* There's an incoming report on the specified fd */
405 if (dev_num < 0 || dev_num >= MAX_DEVICES ||
406 !trig_sensors_per_dev[dev_num]) {
407 ALOGE("Event reported on unexpected iio device %d\n", dev_num);
411 for (s=0; s<MAX_SENSORS; s++)
412 if (sensor_info[s].dev_num == dev_num)
413 for (c=0; c<sensor_info[s].num_channels; c++)
414 expected_size += sensor_info[s].channel[c].size;
416 len = read(device_fd[dev_num], buf, expected_size);
419 ALOGE("Could not read report from iio device %d (%s)\n",
420 dev_num, strerror(errno));
424 ALOGV("Read %d bytes from iio device %d\n", len, dev_num);
426 for (s=0; s<MAX_SENSORS; s++)
427 if (sensor_info[s].dev_num == dev_num) {
430 /* Copy data from device to sensor report buffer */
431 for (c=0; c<sensor_info[s].num_channels; c++) {
433 target = sensor_info[s].report_buffer +
436 source = buf + sensor_info[s].channel[c].offset;
438 size = sensor_info[s].channel[c].size;
440 memcpy(target, source, size);
445 if (sensor_info[s].enable_count) {
446 ALOGV("Sensor %d report available (%d bytes)\n",
449 sensor_info[s].report_pending = 1;
457 static void propagate_sensor_report(int s, struct sensors_event_t* data)
459 /* There's a sensor report pending for this sensor ; transmit it */
461 int catalog_index = sensor_info[s].catalog_index;
462 int sensor_type = sensor_catalog[catalog_index].type;
465 unsigned char* current_sample;
467 memset(data, 0, sizeof(sensors_event_t));
469 data->version = sizeof(sensors_event_t);
471 data->type = sensor_type;
472 data->timestamp = get_timestamp();
474 switch (sensor_type) {
475 case SENSOR_TYPE_ACCELEROMETER: /* m/s^2 */
476 case SENSOR_TYPE_MAGNETIC_FIELD: /* micro-tesla */
477 case SENSOR_TYPE_ORIENTATION: /* degrees */
478 case SENSOR_TYPE_GYROSCOPE: /* radians/s */
482 case SENSOR_TYPE_LIGHT: /* SI lux units */
483 case SENSOR_TYPE_AMBIENT_TEMPERATURE: /* °C */
484 case SENSOR_TYPE_TEMPERATURE: /* °C */
485 case SENSOR_TYPE_PROXIMITY: /* centimeters */
486 case SENSOR_TYPE_PRESSURE: /* hecto-pascal */
487 case SENSOR_TYPE_RELATIVE_HUMIDITY: /* percent */
491 case SENSOR_TYPE_ROTATION_VECTOR:
495 case SENSOR_TYPE_DEVICE_PRIVATE_BASE: /* hidden for now */
500 ALOGE("Unknown sensor type!\n");
505 ALOGV("Sample on sensor %d (type %d):\n", s, sensor_type);
507 /* Take note of current time counter value for rate control purposes */
508 sensor_info[s].last_integration_ts = get_timestamp();
510 /* If we're dealing with a poll-mode sensor */
511 if (!sensor_info[s].num_channels) {
513 /* Read values through sysfs rather than from a report buffer */
514 for (c=0; c<num_fields; c++) {
516 data->data[c] = acquire_immediate_value(s, c);
518 ALOGV("\tfield %d: %f\n", c, data->data[c]);
521 sensor_info[s].ops.finalize(s, data);
525 /* Convert the data into the expected Android-level format */
527 current_sample = sensor_info[s].report_buffer;
529 for (c=0; c<num_fields; c++) {
531 data->data[c] = sensor_info[s].ops.transform
532 (s, c, current_sample);
534 ALOGV("\tfield %d: %f\n", c, data->data[c]);
535 current_sample += sensor_info[s].channel[c].size;
538 sensor_info[s].ops.finalize(s, data);
542 static int get_poll_time (void)
545 int64_t lowest_target_ts;
549 if (!active_poll_sensors)
550 return -1; /* Infinite wait */
552 /* Check if we should schedule a poll-mode sensor event delivery */
554 lowest_target_ts = INT64_MAX;
556 for (s=0; s<sensor_count; s++)
557 if (sensor_info[s].enable_count &&
558 sensor_info[s].sampling_rate &&
559 !sensor_info[s].num_channels) {
560 target_ts = sensor_info[s].last_integration_ts +
561 1000000000LL/sensor_info[s].sampling_rate;
563 if (target_ts < lowest_target_ts)
564 lowest_target_ts = target_ts;
567 if (lowest_target_ts == INT64_MAX)
570 current_ts = get_timestamp();
572 if (lowest_target_ts <= current_ts)
575 return (lowest_target_ts - current_ts)/1000000; /* ms */
579 static void acknowledge_release (void)
581 /* A write to our socket circuit was performed to release epoll */
583 read(poll_socket_pair[0], &buf, 1);
587 int sensor_poll(struct sensors_event_t* data, int count)
593 struct epoll_event ev[MAX_DEVICES];
595 /* Get one or more events from our collection of sensors */
597 return_first_available_sensor_report:
599 /* If there's at least one available report */
600 for (s=0; s<sensor_count; s++)
601 if (sensor_info[s].report_pending) {
604 propagate_sensor_report(s, data);
605 sensor_info[s].report_pending = 0;
606 ALOGV("Report on sensor %d\n", s);
611 /* Keep a minimum time interval between poll operations */
612 delta = (get_timestamp() - last_poll_exit_ts)/1000;
614 if (delta > 0 && delta < POLL_MIN_INTERVAL)
615 usleep(POLL_MIN_INTERVAL - delta);
617 ALOGV("Awaiting sensor data\n");
619 nfds = epoll_wait(poll_fd, ev, MAX_DEVICES, get_poll_time());
621 last_poll_exit_ts = get_timestamp();
624 ALOGI("epoll_wait returned -1 (%s)\n", strerror(errno));
628 ALOGV("%d fds signalled\n", nfds);
630 /* For each of the devices for which a report is available */
631 for (i=0; i<nfds; i++)
632 if (ev[i].events == EPOLLIN) {
633 if (ev[i].data.u32 == INVALID_DEV_NUM) {
634 acknowledge_release();
638 integrate_device_report(ev[i].data.u32);
641 /* It's a good time to invalidate poll-mode sensor values */
642 if (active_poll_sensors)
643 for (s=0; s<sensor_count; s++)
644 if (sensor_info[s].enable_count &&
645 !sensor_info[s].num_channels)
646 sensor_info[s].report_pending = 1;
648 goto return_first_available_sensor_report;
652 int sensor_set_delay(int s, int64_t ns)
654 /* Set the rate at which a specific sensor should report events */
656 /* See Android sensors.h for indication on sensor trigger modes */
658 char sysfs_path[PATH_MAX];
659 char avail_sysfs_path[PATH_MAX];
660 int dev_num = sensor_info[s].dev_num;
661 int i = sensor_info[s].catalog_index;
662 const char *prefix = sensor_catalog[i].tag;
663 int new_sampling_rate;
664 int cur_sampling_rate;
665 int per_sensor_sampling_rate;
666 int per_device_sampling_rate;
672 ALOGE("Rejecting zero delay request on sensor %d\n", s);
676 sprintf(sysfs_path, SENSOR_SAMPLING_PATH, dev_num, prefix);
678 if (sysfs_read_int(sysfs_path, &cur_sampling_rate) != -1) {
679 per_sensor_sampling_rate = 1;
680 per_device_sampling_rate = 0;
682 per_sensor_sampling_rate = 0;
684 sprintf(sysfs_path, DEVICE_SAMPLING_PATH, dev_num);
686 if (sysfs_read_int(sysfs_path, &cur_sampling_rate) != -1)
687 per_device_sampling_rate = 1;
689 per_device_sampling_rate = 0;
692 if (!per_sensor_sampling_rate && !per_device_sampling_rate) {
693 ALOGE("No way to adjust sampling rate on sensor %d\n", s);
697 new_sampling_rate = (int) (1000000000L/ns);
699 if (!new_sampling_rate) {
700 ALOGI("Sub-HZ sampling rate requested on on sensor %d\n", s);
701 new_sampling_rate = 1;
704 sensor_info[s].sampling_rate = new_sampling_rate;
706 /* Coordinate with others active sensors on the same device, if any */
707 if (per_device_sampling_rate)
708 for (n=0; n<sensor_count; n++)
709 if (n != s && sensor_info[n].dev_num == dev_num &&
710 sensor_info[n].enable_count &&
711 sensor_info[n].sampling_rate > new_sampling_rate)
712 new_sampling_rate= sensor_info[n].sampling_rate;
714 /* Check if we have contraints on allowed sampling rates */
716 sprintf(avail_sysfs_path, DEVICE_AVAIL_FREQ_PATH, dev_num);
718 if (sysfs_read_str(avail_sysfs_path, freqs_buf, sizeof(freqs_buf)) > 0){
721 /* Decode allowed sampling rates string, ex: "10 20 50 100" */
723 /* While we're not at the end of the string */
724 while (*cursor && cursor[0]) {
726 /* Decode a single integer value */
729 /* If this matches the selected rate, we're happy */
730 if (new_sampling_rate == n)
734 * If we reached a higher value than the desired rate,
735 * adjust selected rate so it matches the first higher
736 * available one and stop parsing - this makes the
737 * assumption that rates are sorted by increasing value
738 * in the allowed frequencies string.
740 if (n > new_sampling_rate) {
742 "Increasing sampling rate on sensor %d to %d\n",
745 new_sampling_rate = n;
750 while (cursor[0] && !isspace(cursor[0]))
754 while (cursor[0] && isspace(cursor[0]))
759 /* If the desired rate is already active we're all set */
760 if (new_sampling_rate == cur_sampling_rate)
763 ALOGI("Sensor %d sampling rate set to %d\n", s, new_sampling_rate);
765 if (trig_sensors_per_dev[dev_num])
766 enable_buffer(dev_num, 0);
768 sysfs_write_int(sysfs_path, new_sampling_rate);
770 if (trig_sensors_per_dev[dev_num])
771 enable_buffer(dev_num, 1);
773 /* Release the polling loop so an updated timeout value gets used */
774 write(poll_socket_pair[1], "", 1);
781 int allocate_control_data (void)
784 struct epoll_event ev = {0};
786 for (i=0; i<MAX_DEVICES; i++)
789 poll_fd = epoll_create(MAX_DEVICES);
792 ALOGE("Can't create epoll instance for iio sensors!\n");
796 /* Create and add "unblocking" fd to the set of watched fds */
798 if (socketpair(AF_UNIX, SOCK_STREAM, 0, poll_socket_pair) == -1) {
799 ALOGE("Can't create socket pair for iio sensors!\n");
805 ev.data.u32 = INVALID_DEV_NUM;
807 epoll_ctl(poll_fd, EPOLL_CTL_ADD, poll_socket_pair[0], &ev);
813 void delete_control_data (void)