2 * Copyright (C) 2014 Intel Corporation.
7 #include <sys/socket.h>
9 #include <hardware/sensors.h>
11 #include "enumeration.h"
13 #include "transform.h"
15 /* Currently active sensors count, per device */
16 static int poll_sensors_per_dev[MAX_DEVICES]; /* poll-mode sensors */
17 static int trig_sensors_per_dev[MAX_DEVICES]; /* trigger, event based */
19 static int device_fd[MAX_DEVICES]; /* fd on the /dev/iio:deviceX file */
21 static int poll_fd; /* epoll instance covering all enabled sensors */
23 static int poll_socket_pair[2]; /* used to unblock the poll loop */
25 /* Timestamp for the moment when we last exited a poll operation */
26 static int64_t last_poll_exit_ts;
28 static int active_poll_sensors; /* Number of enabled poll-mode sensors */
30 /* Cap the time between poll operations to this, to counter runaway polls */
31 #define POLL_MIN_INTERVAL 10000 /* uS */
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 static void refresh_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
80 char spec_buf[MAX_TYPE_SPEC_LEN];
81 struct datum_info_t* ch_info;
83 char sysfs_path[PATH_MAX];
87 int channel_size_from_index[MAX_SENSORS * MAX_CHANNELS] = { 0 };
88 int sensor_handle_from_index[MAX_SENSORS * MAX_CHANNELS] = { 0 };
89 int channel_number_from_index[MAX_SENSORS * MAX_CHANNELS] = { 0 };
93 /* For each sensor that is linked to this device */
94 for (s=0; s<sensor_count; s++) {
95 if (sensor_info[s].dev_num != dev_num)
98 i = sensor_info[s].catalog_index;
100 /* Read channel status through syfs attributes */
101 for (c=0; c<sensor_info[s].num_channels; c++) {
104 sprintf(sysfs_path, CHANNEL_PATH "%s",
105 sensor_info[s].dev_num,
106 sensor_catalog[i].channel[c].en_path);
108 n = sysfs_read_int(sysfs_path, &ch_enabled);
111 ALOGW( "Failed to read _en flag: %s\n",
116 if (!ch_enabled != 1) {
117 sensor_info[s].channel[c].size = 0;
120 /* Read _type file */
121 sprintf(sysfs_path, CHANNEL_PATH "%s",
122 sensor_info[s].dev_num,
123 sensor_catalog[i].channel[c].type_path);
125 n = sysfs_read_str(sysfs_path, spec_buf,
129 ALOGW( "Failed to read type: %s\n",
134 ch_spec = sensor_info[s].channel[c].type_spec;
136 memcpy(ch_spec, spec_buf, sizeof(spec_buf));
138 ch_info = &sensor_info[s].channel[c].type_info;
140 size = decode_type_spec(ch_spec, ch_info);
142 /* Read _index file */
143 sprintf(sysfs_path, CHANNEL_PATH "%s",
144 sensor_info[s].dev_num,
145 sensor_catalog[i].channel[c].index_path);
147 n = sysfs_read_int(sysfs_path, &ch_index);
150 ALOGW( "Failed to read index: %s\n",
155 if (ch_index >= MAX_SENSORS) {
156 ALOGE("Index out of bounds!: %s\n", sysfs_path);
160 /* Record what this index is about */
162 sensor_handle_from_index [ch_index] = s;
163 channel_number_from_index[ch_index] = c;
164 channel_size_from_index [ch_index] = size;
170 ALOGI("Found %d enabled channels for iio device %d\n", active_channels,
174 * Now that we know which channels are enabled, their sizes and their
175 * ordering, update channels offsets within device report. Note: there
176 * is a possibility that several sensors share the same index, with
177 * their data fields being isolated by masking and shifting as specified
178 * through the real bits and shift values in type attributes. This case
179 * is not currently supported. Also, the code below assumes no hole in
180 * the sequence of indices, so it is dependent on discovery of all
184 for (i=0; i<MAX_SENSORS * MAX_CHANNELS; i++) {
185 s = sensor_handle_from_index[i];
186 c = channel_number_from_index[i];
187 size = channel_size_from_index[i];
192 ALOGI("S%d C%d : offset %d, size %d, type %s\n",
193 s, c, offset, size, sensor_info[s].channel[c].type_spec);
195 sensor_info[s].channel[c].offset = offset;
196 sensor_info[s].channel[c].size = size;
203 int adjust_counters (int s, int enabled)
206 * Adjust counters based on sensor enable action. Return values are:
207 * -1 if there's an inconsistency: abort action in this case
208 * 0 if the operation was completed and we're all set
209 * 1 if we toggled the state of the sensor and there's work left
212 int dev_num = sensor_info[s].dev_num;
214 /* Refcount per sensor, in terms of enable count */
216 ALOGI("Enabling sensor %d (iio device %d: %s)\n",
217 s, dev_num, sensor_info[s].friendly_name);
219 sensor_info[s].enable_count++;
221 if (sensor_info[s].enable_count != 1)
222 return 0; /* The sensor was, and remains, in use */
224 if (sensor_info[s].enable_count == 0)
225 return -1; /* Spurious disable call */
227 ALOGI("Disabling sensor %d (iio device %d: %s)\n", s, dev_num,
228 sensor_info[s].friendly_name);
230 sensor_info[s].enable_count--;
232 if (sensor_info[s].enable_count > 0)
233 return 0; /* The sensor was, and remains, in use */
235 /* Sensor disabled, clear up pending data */
237 sensor_info[s].report_pending = 0;
238 memset(sensor_info[s].report_buffer, 0, MAX_SENSOR_REPORT_SIZE);
241 /* We changed the state of a sensor - adjust per iio device counters */
243 /* If this is a regular event-driven sensor */
244 if (sensor_info[s].num_channels) {
247 trig_sensors_per_dev[dev_num]++;
249 trig_sensors_per_dev[dev_num]--;
255 active_poll_sensors++;
256 poll_sensors_per_dev[dev_num]++;
260 active_poll_sensors--;
261 poll_sensors_per_dev[dev_num]--;
266 int sensor_activate(int s, int enabled)
268 char sysfs_path[PATH_MAX];
269 char device_name[PATH_MAX];
270 char trigger_name[MAX_NAME_SIZE + 16];
272 struct epoll_event ev = {0};
275 int dev_num = sensor_info[s].dev_num;
276 int i = sensor_info[s].catalog_index;
277 int is_poll_sensor = !sensor_info[s].num_channels;
279 ret = adjust_counters(s, enabled);
281 /* If the operation was neutral in terms of state, we're done */
285 if (!is_poll_sensor) {
286 /* Changes have to be made while the buffer is turned off */
287 enable_buffer(dev_num, 0);
289 /* Configure trigger */
290 switch (trig_sensors_per_dev[dev_num]) {
292 setup_trigger(dev_num, "none");
296 sprintf(trigger_name, "%s-dev%d",
297 sensor_info[s].internal_name, dev_num);
299 setup_trigger(dev_num, trigger_name);
303 /* The trigger is already set */
308 * Turn channels associated to this sensor on or off, and update
309 * the channels maps for all sensors associated to this device.
311 for (c=0;c<sensor_info[s].num_channels; c++) {
312 sprintf(sysfs_path, CHANNEL_PATH "%s",
313 sensor_info[s].dev_num,
314 sensor_catalog[i].channel[c].en_path);
316 sysfs_write_int(sysfs_path, enabled);
319 /* If there's at least one sensor left */
320 if (trig_sensors_per_dev[dev_num]) {
321 refresh_sensor_report_maps(dev_num);
322 enable_buffer(dev_num, 1);
327 * Make sure we have a fd on the character device ; conversely, close
328 * the fd if no one is using associated sensor anymore. The assumption
329 * here is that the underlying driver will power on the relevant
330 * hardware block while someone hold a fd on the device.
332 dev_fd = device_fd[dev_num];
335 if (dev_fd != -1 && !poll_sensors_per_dev[dev_num] &&
336 !trig_sensors_per_dev[dev_num]) {
338 * Stop watching this fd. This should be a no-op
339 * in case this fd was not in the poll set.
341 epoll_ctl(poll_fd, EPOLL_CTL_DEL, dev_fd, NULL);
344 device_fd[dev_num] = -1;
350 /* First enabled sensor on this iio device */
351 sprintf(device_name, DEV_FILE_PATH, dev_num);
352 dev_fd = open(device_name, O_RDONLY | O_NONBLOCK);
354 device_fd[dev_num] = dev_fd;
357 ALOGE("Could not open fd on %s (%s)\n",
358 device_name, strerror(errno));
359 adjust_counters(s, 0);
363 ALOGV("Opened %s: fd=%d\n", device_name, dev_fd);
365 if (!is_poll_sensor) {
367 /* Add this iio device fd to the set of watched fds */
369 ev.data.u32 = dev_num;
371 ret = epoll_ctl(poll_fd, EPOLL_CTL_ADD, dev_fd, &ev);
374 ALOGE( "Failed adding %d to poll set (%s)\n",
375 dev_fd, strerror(errno));
379 /* Note: poll-mode fds are not readable */
383 /* Release the polling loop so an updated timeout gets used */
384 write(poll_socket_pair[1], "", 1);
390 static int integrate_device_report(int dev_num)
394 unsigned char buf[MAX_SENSOR_REPORT_SIZE * MAX_SENSORS] = { 0 };
396 unsigned char *target;
397 unsigned char *source;
399 int expected_size = 0;
401 /* There's an incoming report on the specified fd */
403 if (dev_num < 0 || dev_num >= MAX_DEVICES ||
404 !trig_sensors_per_dev[dev_num]) {
405 ALOGE("Event reported on unexpected iio device %d\n", dev_num);
409 for (s=0; s<MAX_SENSORS; s++)
410 if (sensor_info[s].dev_num == dev_num)
411 for (c=0; c<sensor_info[s].num_channels; c++)
412 expected_size += sensor_info[s].channel[c].size;
414 len = read(device_fd[dev_num], buf, expected_size);
417 ALOGE("Could not read report from iio device %d (%s)\n",
418 dev_num, strerror(errno));
422 ALOGV("Read %d bytes from iio device %d\n", len, dev_num);
424 for (s=0; s<MAX_SENSORS; s++)
425 if (sensor_info[s].dev_num == dev_num) {
428 /* Copy data from device to sensor report buffer */
429 for (c=0; c<sensor_info[s].num_channels; c++) {
431 target = sensor_info[s].report_buffer +
434 source = buf + sensor_info[s].channel[c].offset;
436 size = sensor_info[s].channel[c].size;
438 memcpy(target, source, size);
443 if (sensor_info[s].enable_count) {
444 ALOGV("Sensor %d report available (%d bytes)\n",
447 sensor_info[s].report_pending = 1;
455 static float acquire_immediate_value(int s, int c)
457 char sysfs_path[PATH_MAX];
460 int dev_num = sensor_info[s].dev_num;
461 int i = sensor_info[s].catalog_index;
462 const char* raw_path = sensor_catalog[i].channel[c].raw_path;
463 const char* input_path = sensor_catalog[i].channel[c].input_path;
464 float scale = sensor_info[s].scale;
465 float offset = sensor_info[s].offset;
467 /* Acquire a sample value for sensor s / channel c through sysfs */
470 sprintf(sysfs_path, BASE_PATH "%s", dev_num, input_path);
471 ret = sysfs_read_float(sysfs_path, &val);
481 sprintf(sysfs_path, BASE_PATH "%s", dev_num, raw_path);
482 ret = sysfs_read_float(sysfs_path, &val);
487 return (val + offset) * scale;
491 static void propagate_sensor_report(int s, struct sensors_event_t* data)
493 /* There's a sensor report pending for this sensor ; transmit it */
495 int catalog_index = sensor_info[s].catalog_index;
496 int sensor_type = sensor_catalog[catalog_index].type;
499 unsigned char* current_sample;
501 memset(data, 0, sizeof(sensors_event_t));
503 data->version = sizeof(sensors_event_t);
505 data->type = sensor_type;
506 data->timestamp = get_timestamp();
508 switch (sensor_type) {
509 case SENSOR_TYPE_ACCELEROMETER: /* m/s^2 */
510 case SENSOR_TYPE_MAGNETIC_FIELD: /* micro-tesla */
511 case SENSOR_TYPE_ORIENTATION: /* degrees */
512 case SENSOR_TYPE_GYROSCOPE: /* radians/s */
516 case SENSOR_TYPE_LIGHT: /* SI lux units */
517 case SENSOR_TYPE_AMBIENT_TEMPERATURE: /* °C */
518 case SENSOR_TYPE_TEMPERATURE: /* °C */
519 case SENSOR_TYPE_PROXIMITY: /* centimeters */
520 case SENSOR_TYPE_PRESSURE: /* hecto-pascal */
521 case SENSOR_TYPE_RELATIVE_HUMIDITY: /* percent */
525 case SENSOR_TYPE_ROTATION_VECTOR:
529 case SENSOR_TYPE_DEVICE_PRIVATE_BASE: /* hidden for now */
534 ALOGE("Unknown sensor type!\n");
539 ALOGV("Sample on sensor %d (type %d):\n", s, sensor_type);
541 /* Take note of current time counter value for rate control purposes */
542 sensor_info[s].last_integration_ts = get_timestamp();
544 /* If we're dealing with a poll-mode sensor */
545 if (!sensor_info[s].num_channels) {
547 /* Read values through sysfs rather than from a report buffer */
548 for (c=0; c<num_fields; c++) {
550 data->data[c] = acquire_immediate_value(s, c);
552 ALOGV("\tfield %d: %f\n", c, data->data[c]);
555 finalize_sample(s, data);
559 /* Convert the data into the expected Android-level format */
561 current_sample = sensor_info[s].report_buffer;
563 for (c=0; c<num_fields; c++) {
565 data->data[c] = transform_sample(s, c, current_sample);
567 ALOGV("\tfield %d: %f\n", c, data->data[c]);
568 current_sample += sensor_info[s].channel[c].size;
571 finalize_sample(s, data);
575 static int get_poll_time (void)
578 int64_t lowest_target_ts;
582 if (!active_poll_sensors)
583 return -1; /* Infinite wait */
585 /* Check if we should schedule a poll-mode sensor event delivery */
587 lowest_target_ts = INT64_MAX;
589 for (s=0; s<sensor_count; s++)
590 if (sensor_info[s].enable_count &&
591 sensor_info[s].sampling_rate &&
592 !sensor_info[s].num_channels) {
593 target_ts = sensor_info[s].last_integration_ts +
594 1000000000LL/sensor_info[s].sampling_rate;
596 if (target_ts < lowest_target_ts)
597 lowest_target_ts = target_ts;
600 if (lowest_target_ts == INT64_MAX)
603 current_ts = get_timestamp();
605 if (lowest_target_ts <= current_ts)
608 return (lowest_target_ts - current_ts)/1000000; /* ms */
612 static void acknowledge_release (void)
614 /* A write to our socket circuit was performed to release epoll */
616 read(poll_socket_pair[0], &buf, 1);
620 int sensor_poll(struct sensors_event_t* data, int count)
626 struct epoll_event ev[MAX_DEVICES];
628 /* Get one or more events from our collection of sensors */
630 return_first_available_sensor_report:
632 /* If there's at least one available report */
633 for (s=0; s<sensor_count; s++)
634 if (sensor_info[s].report_pending) {
637 propagate_sensor_report(s, data);
638 sensor_info[s].report_pending = 0;
639 ALOGV("Report on sensor %d\n", s);
644 /* Keep a minimum time interval between poll operations */
645 delta = (get_timestamp() - last_poll_exit_ts)/1000;
647 if (delta > 0 && delta < POLL_MIN_INTERVAL)
648 usleep(POLL_MIN_INTERVAL - delta);
650 ALOGV("Awaiting sensor data\n");
652 nfds = epoll_wait(poll_fd, ev, MAX_DEVICES, get_poll_time());
654 last_poll_exit_ts = get_timestamp();
657 ALOGI("epoll_wait returned -1 (%s)\n", strerror(errno));
661 ALOGV("%d fds signalled\n", nfds);
663 /* For each of the devices for which a report is available */
664 for (i=0; i<nfds; i++)
665 if (ev[i].events == EPOLLIN) {
666 if (ev[i].data.u32 == INVALID_DEV_NUM) {
667 acknowledge_release();
671 integrate_device_report(ev[i].data.u32);
674 /* It's a good time to invalidate poll-mode sensor values */
675 if (active_poll_sensors)
676 for (s=0; s<sensor_count; s++)
677 if (sensor_info[s].enable_count &&
678 !sensor_info[s].num_channels)
679 sensor_info[s].report_pending = 1;
681 goto return_first_available_sensor_report;
685 int sensor_set_delay(int s, int64_t ns)
687 /* Set the rate at which a specific sensor should report events */
689 /* See Android sensors.h for indication on sensor trigger modes */
691 char sysfs_path[PATH_MAX];
692 int dev_num = sensor_info[s].dev_num;
693 int i = sensor_info[s].catalog_index;
694 const char *prefix = sensor_catalog[i].tag;
695 int new_sampling_rate;
696 int cur_sampling_rate;
699 ALOGE("Rejecting zero delay request on sensor %d\n", s);
703 new_sampling_rate = (int) (1000000000L/ns);
705 if (!new_sampling_rate) {
706 ALOGI("Sub-HZ sampling rate requested on on sensor %d\n", s);
707 new_sampling_rate = 1;
710 sprintf(sysfs_path, COMMON_SAMPLING_PATH, dev_num, prefix);
712 if (sysfs_read_int(sysfs_path, &cur_sampling_rate) != -1)
713 if (new_sampling_rate != cur_sampling_rate) {
714 ALOGI( "Sensor %d sampling rate set to %d\n",
715 s, new_sampling_rate);
717 if (trig_sensors_per_dev[dev_num])
718 enable_buffer(dev_num, 0);
720 sysfs_write_int(sysfs_path, new_sampling_rate);
722 if (trig_sensors_per_dev[dev_num])
723 enable_buffer(dev_num, 1);
726 sensor_info[s].sampling_rate = new_sampling_rate;
728 /* Release the polling loop so an updated timeout value gets used */
729 write(poll_socket_pair[1], "", 1);
735 int allocate_control_data (void)
738 struct epoll_event ev = {0};
740 for (i=0; i<MAX_DEVICES; i++)
743 poll_fd = epoll_create(MAX_DEVICES);
746 ALOGE("Can't create epoll instance for iio sensors!\n");
750 /* Create and add "unblocking" fd to the set of watched fds */
752 if (socketpair(AF_UNIX, SOCK_STREAM, 0, poll_socket_pair) == -1) {
753 ALOGE("Can't create socket pair for iio sensors!\n");
759 ev.data.u32 = INVALID_DEV_NUM;
761 epoll_ctl(poll_fd, EPOLL_CTL_ADD, poll_socket_pair[0], &ev);
767 void delete_control_data (void)