+static int get_field_count (int s)
+{
+ int catalog_index = sensor_info[s].catalog_index;
+ int sensor_type = sensor_catalog[catalog_index].type;
+
+ switch (sensor_type) {
+ case SENSOR_TYPE_ACCELEROMETER: /* m/s^2 */
+ case SENSOR_TYPE_MAGNETIC_FIELD: /* micro-tesla */
+ case SENSOR_TYPE_ORIENTATION: /* degrees */
+ case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
+ case SENSOR_TYPE_GYROSCOPE: /* radians/s */
+ return 3;
+
+ case SENSOR_TYPE_LIGHT: /* SI lux units */
+ case SENSOR_TYPE_AMBIENT_TEMPERATURE: /* °C */
+ case SENSOR_TYPE_TEMPERATURE: /* °C */
+ case SENSOR_TYPE_PROXIMITY: /* centimeters */
+ case SENSOR_TYPE_PRESSURE: /* hecto-pascal */
+ case SENSOR_TYPE_RELATIVE_HUMIDITY: /* percent */
+ return 1;
+
+ case SENSOR_TYPE_ROTATION_VECTOR:
+ return 4;
+
+ default:
+ ALOGE("Unknown sensor type!\n");
+ return 0; /* Drop sample */
+ }
+}
+
+
+static void time_add(struct timespec *out, struct timespec *in, int64_t ns)
+{
+ int64_t target_ts = 1000000000LL * in->tv_sec + in->tv_nsec + ns;
+
+ out->tv_sec = target_ts / 1000000000;
+ out->tv_nsec = target_ts % 1000000000;
+}
+
+
+static void* acquisition_routine (void* param)
+{
+ /*
+ * Data acquisition routine run in a dedicated thread, covering a single
+ * sensor. This loop will periodically retrieve sampling data through
+ * sysfs, then package it as a sample and transfer it to our master poll
+ * loop through a report fd. Checks for a cancellation signal quite
+ * frequently, as the thread may be disposed of at any time. Note that
+ * Bionic does not provide pthread_cancel / pthread_testcancel...
+ */
+
+ int s = (int) (size_t) param;
+ int num_fields;
+ struct sensors_event_t data = {0};
+ int c;
+ int ret;
+ struct timespec entry_time;
+ struct timespec target_time;
+ int64_t period;
+
+ ALOGI("Entering data acquisition thread S%d (%s): rate(%f), minDelay(%ld), maxDelay(%ld)\n",
+ s, sensor_info[s].friendly_name, sensor_info[s].sampling_rate,
+ sensor_desc[s].minDelay, sensor_desc[s].maxDelay);
+
+ if (s < 0 || s >= sensor_count) {
+ ALOGE("Invalid sensor handle!\n");
+ return NULL;
+ }
+
+ if (!sensor_info[s].sampling_rate) {
+ ALOGE("Zero rate in acquisition routine for sensor %d\n", s);
+ return NULL;
+ }
+
+ num_fields = get_field_count(s);
+
+ /*
+ * Each condition variable is associated to a mutex that has to be
+ * locked by the thread that's waiting on it. We use these condition
+ * variables to get the acquisition threads out of sleep quickly after
+ * the sampling rate is adjusted, or the sensor is disabled.
+ */
+ pthread_mutex_lock(&thread_release_mutex[s]);
+
+ while (1) {
+ /* Pinpoint the moment we start sampling */
+ clock_gettime(CLOCK_REALTIME, &entry_time);
+
+ ALOGV("Acquiring sample data for sensor %d through sysfs\n", s);
+
+ /* Read values through sysfs */
+ for (c=0; c<num_fields; c++) {
+ data.data[c] = acquire_immediate_value(s, c);
+
+ /* Check and honor termination requests */
+ if (sensor_info[s].thread_data_fd[1] == -1)
+ goto exit;
+
+ ALOGV("\tfield %d: %f\n", c, data.data[c]);
+
+ }
+
+ /* If the sample looks good */
+ if (sensor_info[s].ops.finalize(s, &data)) {
+
+ /* Pipe it for transmission to poll loop */
+ ret = write( sensor_info[s].thread_data_fd[1],
+ data.data,
+ num_fields * sizeof(float));
+ }
+
+ /* Check and honor termination requests */
+ if (sensor_info[s].thread_data_fd[1] == -1)
+ goto exit;
+
+
+ period = (int64_t) (1000000000.0/ sensor_info[s].sampling_rate);
+ period = period * OVERHEAD_THRESHOLD;
+ time_add(&target_time, &entry_time, period);
+
+ /*
+ * Wait until the sampling time elapses, or a rate change is
+ * signaled, or a thread exit is requested.
+ */
+ ret = pthread_cond_timedwait( &thread_release_cond[s],
+ &thread_release_mutex[s],
+ &target_time);
+
+ /* Check and honor termination requests */
+ if (sensor_info[s].thread_data_fd[1] == -1)
+ goto exit;
+ }
+
+exit:
+ ALOGV("Acquisition thread for S%d exiting\n", s);
+ pthread_mutex_unlock(&thread_release_mutex[s]);
+ pthread_exit(0);
+ return NULL;
+}
+
+
+static void start_acquisition_thread (int s)
+{
+ int incoming_data_fd;
+ int ret;
+
+ struct epoll_event ev = {0};
+
+ ALOGV("Initializing acquisition context for sensor %d\n", s);
+
+ /* Create condition variable and mutex for quick thread release */
+ ret = pthread_cond_init(&thread_release_cond[s], NULL);
+ ret = pthread_mutex_init(&thread_release_mutex[s], NULL);
+
+ /* Create a pipe for inter thread communication */
+ ret = pipe(sensor_info[s].thread_data_fd);
+
+ incoming_data_fd = sensor_info[s].thread_data_fd[0];
+
+ ev.events = EPOLLIN;
+ ev.data.u32 = THREAD_REPORT_TAG_BASE + s;
+
+ /* Add incoming side of pipe to our poll set, with a suitable tag */
+ ret = epoll_ctl(poll_fd, EPOLL_CTL_ADD, incoming_data_fd , &ev);
+
+ /* Create and start worker thread */
+ ret = pthread_create( &sensor_info[s].acquisition_thread,
+ NULL,
+ acquisition_routine,
+ (void*) (size_t) s);
+}
+
+
+static void stop_acquisition_thread (int s)
+{
+ int incoming_data_fd = sensor_info[s].thread_data_fd[0];
+ int outgoing_data_fd = sensor_info[s].thread_data_fd[1];
+
+ ALOGV("Tearing down acquisition context for sensor %d\n", s);
+
+ /* Delete the incoming side of the pipe from our poll set */
+ epoll_ctl(poll_fd, EPOLL_CTL_DEL, incoming_data_fd, NULL);
+
+ /* Mark the pipe ends as invalid ; that's a cheap exit flag */
+ sensor_info[s].thread_data_fd[0] = -1;
+ sensor_info[s].thread_data_fd[1] = -1;
+
+ /* Close both sides of our pipe */
+ close(incoming_data_fd);
+ close(outgoing_data_fd);
+
+ /* Stop acquisition thread and clean up thread handle */
+ pthread_cond_signal(&thread_release_cond[s]);
+ pthread_join(sensor_info[s].acquisition_thread, NULL);
+
+ /* Clean up our sensor descriptor */
+ sensor_info[s].acquisition_thread = -1;
+
+ /* Delete condition variable and mutex */
+ pthread_cond_destroy(&thread_release_cond[s]);
+ pthread_mutex_destroy(&thread_release_mutex[s]);
+}
+
+